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.wpilibj.smartdashboard;
006
007import edu.wpi.first.networktables.DoublePublisher;
008import edu.wpi.first.networktables.NetworkTable;
009import java.util.HashMap;
010import java.util.Map;
011
012/**
013 * Root Mechanism2d node.
014 *
015 * <p>A root is the anchor point of other nodes (such as ligaments).
016 *
017 * <p>Do not create objects of this class directly! Obtain instances from the {@link
018 * Mechanism2d#getRoot(String, double, double)} factory method.
019 *
020 * <p>Append other nodes by using {@link #append(MechanismObject2d)}.
021 */
022public final class MechanismRoot2d implements AutoCloseable {
023  private final String m_name;
024  private NetworkTable m_table;
025  private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
026  private double m_x;
027  private DoublePublisher m_xPub;
028  private double m_y;
029  private DoublePublisher m_yPub;
030
031  /**
032   * Package-private constructor for roots.
033   *
034   * @param name name
035   * @param x x coordinate of root (provide only when constructing a root node)
036   * @param y y coordinate of root (provide only when constructing a root node)
037   */
038  MechanismRoot2d(String name, double x, double y) {
039    m_name = name;
040    m_x = x;
041    m_y = y;
042  }
043
044  @Override
045  public void close() {
046    if (m_xPub != null) {
047      m_xPub.close();
048    }
049    if (m_yPub != null) {
050      m_yPub.close();
051    }
052    for (MechanismObject2d obj : m_objects.values()) {
053      obj.close();
054    }
055  }
056
057  /**
058   * Append a Mechanism object that is based on this one.
059   *
060   * @param <T> The object type.
061   * @param object the object to add.
062   * @return the object given as a parameter, useful for variable assignments and call chaining.
063   * @throws UnsupportedOperationException if the object's name is already used - object names must
064   *     be unique.
065   */
066  public synchronized <T extends MechanismObject2d> T append(T object) {
067    if (m_objects.containsKey(object.getName())) {
068      throw new UnsupportedOperationException("Mechanism object names must be unique!");
069    }
070    m_objects.put(object.getName(), object);
071    if (m_table != null) {
072      object.update(m_table.getSubTable(object.getName()));
073    }
074    return object;
075  }
076
077  /**
078   * Set the root's position.
079   *
080   * @param x new x coordinate
081   * @param y new y coordinate
082   */
083  public synchronized void setPosition(double x, double y) {
084    m_x = x;
085    m_y = y;
086    flush();
087  }
088
089  synchronized void update(NetworkTable table) {
090    m_table = table;
091    if (m_xPub != null) {
092      m_xPub.close();
093    }
094    m_xPub = m_table.getDoubleTopic("x").publish();
095    if (m_yPub != null) {
096      m_yPub.close();
097    }
098    m_yPub = m_table.getDoubleTopic("y").publish();
099    flush();
100    for (MechanismObject2d obj : m_objects.values()) {
101      obj.update(m_table.getSubTable(obj.getName()));
102    }
103  }
104
105  public String getName() {
106    return m_name;
107  }
108
109  private void flush() {
110    if (m_xPub != null) {
111      m_xPub.set(m_x);
112    }
113    if (m_yPub != null) {
114      m_yPub.set(m_y);
115    }
116  }
117}