[wpimath] Add remaining struct and protobuf implementations (#5953)

wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.h

@@ -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.
+
+#pragma once
+
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/proto/MatrixProto.h"
+#include "frc/system/LinearSystem.h"
+
+template <int States, int Inputs, int Outputs>
+struct wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::LinearSystem<States, Inputs, Outputs> Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::LinearSystem<States, Inputs, Outputs>& value);
+};
+
+#include "frc/system/proto/LinearSystemProto.inc"

wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc

@@ -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.
+
+#pragma once
+
+#include <stdexcept>
+
+#include <fmt/format.h>
+#include <wpi/ProtoHelper.h>
+
+#include "frc/system/proto/LinearSystemProto.h"
+#include "system.pb.h"
+
+template <int States, int Inputs, int Outputs>
+google::protobuf::Message*
+wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::New(
+    google::protobuf::Arena* arena) {
+  return wpi::CreateMessage<wpi::proto::ProtobufLinearSystem>(arena);
+}
+
+template <int States, int Inputs, int Outputs>
+frc::LinearSystem<States, Inputs, Outputs>
+wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufLinearSystem*>(&msg);
+  if (m->num_states() != States || m->num_inputs() != Inputs ||
+      m->num_outputs() != Outputs) {
+    throw std::invalid_argument(fmt::format(
+        "Tried to unpack message with {} states and {} inputs and {} outputs "
+        "into LinearSystem with {} states and {} inputs and {} outputs",
+        m->num_states(), m->num_inputs(), m->num_outputs(), States, Inputs,
+        Outputs));
+  }
+  return frc::LinearSystem<States, Inputs, Outputs>{
+      wpi::UnpackProtobuf<frc::Matrixd<States, States>>(m->a()),
+      wpi::UnpackProtobuf<frc::Matrixd<States, Inputs>>(m->b()),
+      wpi::UnpackProtobuf<frc::Matrixd<Outputs, States>>(m->c()),
+      wpi::UnpackProtobuf<frc::Matrixd<Outputs, Inputs>>(m->d())};
+}
+
+template <int States, int Inputs, int Outputs>
+void wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
+    google::protobuf::Message* msg,
+    const frc::LinearSystem<States, Inputs, Outputs>& value) {
+  auto m = static_cast<wpi::proto::ProtobufLinearSystem*>(msg);
+  m->set_num_states(States);
+  m->set_num_inputs(Inputs);
+  m->set_num_outputs(Outputs);
+  wpi::PackProtobuf(m->mutable_a(), value.A());
+  wpi::PackProtobuf(m->mutable_b(), value.B());
+  wpi::PackProtobuf(m->mutable_c(), value.C());
+  wpi::PackProtobuf(m->mutable_d(), value.D());
+}

wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h

@@ -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.
+
+#pragma once
+
+#include <fmt/format.h>
+#include <wpi/ct_string.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/struct/MatrixStruct.h"
+#include "frc/system/LinearSystem.h"
+
+template <int States, int Inputs, int Outputs>
+struct wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>> {
+  static constexpr ct_string kTypeName =
+      wpi::Concat("LinearSystem__"_ct_string, wpi::NumToCtString<States>(),
+                  "_"_ct_string, wpi::NumToCtString<Inputs>(), "_"_ct_string,
+                  wpi::NumToCtString<Outputs>());
+  static constexpr std::string_view GetTypeName() { return kTypeName; }
+  static constexpr size_t GetSize() {
+    return wpi::Struct<frc::Matrixd<States, States>>::GetSize() +
+           wpi::Struct<frc::Matrixd<States, Inputs>>::GetSize() +
+           wpi::Struct<frc::Matrixd<Outputs, States>>::GetSize() +
+           wpi::Struct<frc::Matrixd<Outputs, Inputs>>::GetSize();
+  }
+  static constexpr ct_string kSchema = wpi::Concat(
+      wpi::Struct<frc::Matrixd<States, States>>::kTypeName, " a;"_ct_string,
+      wpi::Struct<frc::Matrixd<States, Inputs>>::kTypeName, " b;"_ct_string,
+      wpi::Struct<frc::Matrixd<Outputs, States>>::kTypeName, " c;"_ct_string,
+      wpi::Struct<frc::Matrixd<Outputs, Inputs>>::kTypeName, " d"_ct_string);
+  static constexpr std::string_view GetSchema() { return kSchema; }
+
+  static frc::LinearSystem<States, Inputs, Outputs> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::LinearSystem<States, Inputs, Outputs>& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Matrixd<States, States>>(fn);
+    wpi::ForEachStructSchema<frc::Matrixd<States, Inputs>>(fn);
+    wpi::ForEachStructSchema<frc::Matrixd<Outputs, States>>(fn);
+    wpi::ForEachStructSchema<frc::Matrixd<Outputs, Inputs>>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::LinearSystem<4, 3, 2>>);
+static_assert(wpi::HasNestedStruct<frc::LinearSystem<4, 3, 2>>);
+static_assert(wpi::StructSerializable<frc::LinearSystem<2, 3, 4>>);
+static_assert(wpi::HasNestedStruct<frc::LinearSystem<2, 3, 4>>);
+
+#include "frc/system/struct/LinearSystemStruct.inc"

wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.inc

@@ -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.
+
+#pragma once
+
+#include "frc/system/struct/LinearSystemStruct.h"
+
+template <int States, int Inputs, int Outputs>
+frc::LinearSystem<States, Inputs, Outputs>
+wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
+    std::span<const uint8_t> data) {
+  constexpr size_t kAOff = 0;
+  constexpr size_t kBOff =
+      kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
+  constexpr size_t kCOff =
+      kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
+  constexpr size_t kDOff =
+      kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
+  return frc::LinearSystem<States, Inputs, Outputs>{
+      wpi::UnpackStruct<frc::Matrixd<States, States>, kAOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<States, Inputs>, kBOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<Outputs, States>, kCOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<Outputs, Inputs>, kDOff>(data)};
+}
+
+template <int States, int Inputs, int Outputs>
+void wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
+    std::span<uint8_t> data,
+    const frc::LinearSystem<States, Inputs, Outputs>& value) {
+  constexpr size_t kAOff = 0;
+  constexpr size_t kBOff =
+      kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
+  constexpr size_t kCOff =
+      kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
+  constexpr size_t kDOff =
+      kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
+  wpi::PackStruct<kAOff>(data, value.A());
+  wpi::PackStruct<kBOff>(data, value.B());
+  wpi::PackStruct<kCOff>(data, value.C());
+  wpi::PackStruct<kDOff>(data, value.D());
+}