001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.system.proto;
006
007import edu.wpi.first.math.Matrix;
008import edu.wpi.first.math.Nat;
009import edu.wpi.first.math.Num;
010import edu.wpi.first.math.proto.System.ProtobufLinearSystem;
011import edu.wpi.first.math.proto.Wpimath.ProtobufMatrix;
012import edu.wpi.first.math.system.LinearSystem;
013import edu.wpi.first.util.protobuf.Protobuf;
014import us.hebi.quickbuf.Descriptors.Descriptor;
015
016public final class LinearSystemProto<States extends Num, Inputs extends Num, Outputs extends Num>
017    implements Protobuf<LinearSystem<States, Inputs, Outputs>, ProtobufLinearSystem> {
018  private final Nat<States> m_states;
019  private final Nat<Inputs> m_inputs;
020  private final Nat<Outputs> m_outputs;
021  private final Protobuf<Matrix<States, States>, ProtobufMatrix> m_AProto;
022  private final Protobuf<Matrix<States, Inputs>, ProtobufMatrix> m_BProto;
023  private final Protobuf<Matrix<Outputs, States>, ProtobufMatrix> m_CProto;
024  private final Protobuf<Matrix<Outputs, Inputs>, ProtobufMatrix> m_DProto;
025
026  /**
027   * Constructs the {@link Protobuf} implementation.
028   *
029   * @param states The number of states of the linear systems this serializer processes.
030   * @param inputs The number of inputs of the linear systems this serializer processes.
031   * @param outputs The number of outputs of the linear systems this serializer processes.
032   */
033  public LinearSystemProto(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
034    m_states = states;
035    m_inputs = inputs;
036    m_outputs = outputs;
037    m_AProto = Matrix.getProto(states, states);
038    m_BProto = Matrix.getProto(states, inputs);
039    m_CProto = Matrix.getProto(outputs, states);
040    m_DProto = Matrix.getProto(outputs, inputs);
041  }
042
043  @Override
044  public Class<LinearSystem<States, Inputs, Outputs>> getTypeClass() {
045    @SuppressWarnings("unchecked")
046    var clazz = (Class<LinearSystem<States, Inputs, Outputs>>) (Class<?>) LinearSystem.class;
047    return clazz;
048  }
049
050  @Override
051  public Descriptor getDescriptor() {
052    return ProtobufLinearSystem.getDescriptor();
053  }
054
055  @Override
056  public ProtobufLinearSystem createMessage() {
057    return ProtobufLinearSystem.newInstance();
058  }
059
060  @Override
061  public LinearSystem<States, Inputs, Outputs> unpack(ProtobufLinearSystem msg) {
062    if (msg.getNumStates() != m_states.getNum()
063        || msg.getNumInputs() != m_inputs.getNum()
064        || msg.getNumOutputs() != m_outputs.getNum()) {
065      throw new IllegalArgumentException(
066          "Tried to unpack msg "
067              + msg
068              + " with "
069              + msg.getNumStates()
070              + " states and "
071              + msg.getNumInputs()
072              + " inputs and "
073              + msg.getNumOutputs()
074              + " outputs into LinearSystem with "
075              + m_states.getNum()
076              + " states "
077              + m_inputs.getNum()
078              + " inputs "
079              + m_outputs.getNum()
080              + " outputs");
081    }
082    return new LinearSystem<>(
083        m_AProto.unpack(msg.getA()),
084        m_BProto.unpack(msg.getB()),
085        m_CProto.unpack(msg.getC()),
086        m_DProto.unpack(msg.getD()));
087  }
088
089  @Override
090  public void pack(ProtobufLinearSystem msg, LinearSystem<States, Inputs, Outputs> value) {
091    msg.setNumStates(m_states.getNum())
092        .setNumInputs(m_inputs.getNum())
093        .setNumOutputs(m_outputs.getNum());
094    m_AProto.pack(msg.getMutableA(), value.getA());
095    m_BProto.pack(msg.getMutableB(), value.getB());
096    m_CProto.pack(msg.getMutableC(), value.getC());
097    m_DProto.pack(msg.getMutableD(), value.getD());
098  }
099}