From 830c0c5c2fc7c3dc1e407439db7b70a90d757556 Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Fri, 21 Jan 2022 15:47:44 -0800
Subject: [PATCH] [wpilib] MechanismLigament2d: Add getters for color and line
 weight (#3947)

Also add missing locking in C++.
---
 .../smartdashboard/MechanismLigament2d.cpp    | 22 +++++++++++
 .../frc/smartdashboard/MechanismLigament2d.h  | 16 +++++++-
 .../smartdashboard/MechanismLigament2d.java   | 38 +++++++++++++++++++
 3 files changed, 75 insertions(+), 1 deletion(-)

diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
index 6d65600af3..b49bc9954b 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
@@ -49,7 +49,20 @@ void MechanismLigament2d::SetLineWeight(double lineWidth) {
   Flush();
 }
 
+Color8Bit MechanismLigament2d::GetColor() {
+  std::scoped_lock lock(m_mutex);
+  if (m_colorEntry) {
+    auto color = m_colorEntry.GetString("");
+    std::strncpy(m_color, color.c_str(), sizeof(m_color));
+    m_color[sizeof(m_color) - 1] = '\0';
+  }
+  unsigned int r = 0, g = 0, b = 0;
+  std::sscanf(m_color, "#%02X%02X%02X", &r, &g, &b);
+  return {static_cast<int>(r), static_cast<int>(g), static_cast<int>(b)};
+}
+
 double MechanismLigament2d::GetAngle() {
+  std::scoped_lock lock(m_mutex);
   if (m_angleEntry) {
     m_angle = m_angleEntry.GetDouble(0.0);
   }
@@ -57,12 +70,21 @@ double MechanismLigament2d::GetAngle() {
 }
 
 double MechanismLigament2d::GetLength() {
+  std::scoped_lock lock(m_mutex);
   if (m_lengthEntry) {
     m_length = m_lengthEntry.GetDouble(0.0);
   }
   return m_length;
 }
 
+double MechanismLigament2d::GetLineWeight() {
+  std::scoped_lock lock(m_mutex);
+  if (m_weightEntry) {
+    m_weight = m_weightEntry.GetDouble(0.0);
+  }
+  return m_weight;
+}
+
 void MechanismLigament2d::SetLength(double length) {
   std::scoped_lock lock(m_mutex);
   m_length = length;
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
index 70abaa6db3..f1bebb9a67 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
@@ -34,7 +34,14 @@ class MechanismLigament2d : public MechanismObject2d {
    *
    * @param color the color of the line
    */
-  void SetColor(const frc::Color8Bit& color);
+  void SetColor(const Color8Bit& color);
+
+  /**
+   * Get the ligament color.
+   *
+   * @return the color of the line
+   */
+  Color8Bit GetColor();
 
   /**
    * Set the ligament's length.
@@ -71,6 +78,13 @@ class MechanismLigament2d : public MechanismObject2d {
    */
   void SetLineWeight(double lineWidth);
 
+  /**
+   * Get the line thickness.
+   *
+   * @return the line thickness
+   */
+  double GetLineWeight();
+
  protected:
   void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) override;
 
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
index 774ab29e86..88236debe5 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
@@ -117,6 +117,32 @@ public class MechanismLigament2d extends MechanismObject2d {
     flush();
   }
 
+  /**
+   * Get the ligament color.
+   *
+   * @return the color of the line
+   */
+  public synchronized Color8Bit getColor() {
+    if (m_colorEntry != null) {
+      m_color = m_colorEntry.getString("");
+    }
+    int r = 0;
+    int g = 0;
+    int b = 0;
+    if (m_color.length() == 7 && m_color.charAt(0) == '#') {
+      try {
+        r = Integer.parseInt(m_color.substring(1, 3), 16);
+        g = Integer.parseInt(m_color.substring(3, 5), 16);
+        b = Integer.parseInt(m_color.substring(5, 7), 16);
+      } catch (NumberFormatException e) {
+        r = 0;
+        g = 0;
+        b = 0;
+      }
+    }
+    return new Color8Bit(r, g, b);
+  }
+
   /**
    * Set the line thickness.
    *
@@ -127,6 +153,18 @@ public class MechanismLigament2d extends MechanismObject2d {
     flush();
   }
 
+  /**
+   * Get the line thickness.
+   *
+   * @return the line thickness
+   */
+  public synchronized double getLineWeight() {
+    if (m_weightEntry != null) {
+      m_weight = m_weightEntry.getDouble(0.0);
+    }
+    return m_weight;
+  }
+
   @Override
   protected void updateEntries(NetworkTable table) {
     table.getEntry(".type").setString("line");