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.EntryListenerFlags;
008import edu.wpi.first.networktables.NTSendableBuilder;
009import edu.wpi.first.networktables.NetworkTable;
010import edu.wpi.first.networktables.NetworkTableEntry;
011import edu.wpi.first.networktables.NetworkTableValue;
012import edu.wpi.first.util.function.BooleanConsumer;
013import java.util.ArrayList;
014import java.util.List;
015import java.util.function.BooleanSupplier;
016import java.util.function.Consumer;
017import java.util.function.DoubleConsumer;
018import java.util.function.DoubleSupplier;
019import java.util.function.Function;
020import java.util.function.Supplier;
021
022public class SendableBuilderImpl implements NTSendableBuilder {
023  private static class Property {
024    Property(NetworkTable table, String key) {
025      m_entry = table.getEntry(key);
026    }
027
028    @Override
029    @SuppressWarnings("NoFinalizer")
030    protected synchronized void finalize() {
031      stopListener();
032    }
033
034    void startListener() {
035      if (m_entry.isValid() && m_listener == 0 && m_createListener != null) {
036        m_listener = m_createListener.apply(m_entry);
037      }
038    }
039
040    void stopListener() {
041      if (m_entry.isValid() && m_listener != 0) {
042        m_entry.removeListener(m_listener);
043        m_listener = 0;
044      }
045    }
046
047    final NetworkTableEntry m_entry;
048    int m_listener;
049    Consumer<NetworkTableEntry> m_update;
050    Function<NetworkTableEntry, Integer> m_createListener;
051  }
052
053  private final List<Property> m_properties = new ArrayList<>();
054  private Runnable m_safeState;
055  private final List<Runnable> m_updateTables = new ArrayList<>();
056  private NetworkTable m_table;
057  private NetworkTableEntry m_controllableEntry;
058  private boolean m_actuator;
059
060  /**
061   * Set the network table. Must be called prior to any Add* functions being called.
062   *
063   * @param table Network table
064   */
065  public void setTable(NetworkTable table) {
066    m_table = table;
067    m_controllableEntry = table.getEntry(".controllable");
068  }
069
070  /**
071   * Get the network table.
072   *
073   * @return The network table
074   */
075  @Override
076  public NetworkTable getTable() {
077    return m_table;
078  }
079
080  /**
081   * Return whether this sendable has an associated table.
082   *
083   * @return True if it has a table, false if not.
084   */
085  @Override
086  public boolean isPublished() {
087    return m_table != null;
088  }
089
090  /**
091   * Return whether this sendable should be treated as an actuator.
092   *
093   * @return True if actuator, false if not.
094   */
095  public boolean isActuator() {
096    return m_actuator;
097  }
098
099  /** Update the network table values by calling the getters for all properties. */
100  @Override
101  public void update() {
102    for (Property property : m_properties) {
103      if (property.m_update != null) {
104        property.m_update.accept(property.m_entry);
105      }
106    }
107    for (Runnable updateTable : m_updateTables) {
108      updateTable.run();
109    }
110  }
111
112  /** Hook setters for all properties. */
113  public void startListeners() {
114    for (Property property : m_properties) {
115      property.startListener();
116    }
117    if (m_controllableEntry != null) {
118      m_controllableEntry.setBoolean(true);
119    }
120  }
121
122  /** Unhook setters for all properties. */
123  public void stopListeners() {
124    for (Property property : m_properties) {
125      property.stopListener();
126    }
127    if (m_controllableEntry != null) {
128      m_controllableEntry.setBoolean(false);
129    }
130  }
131
132  /**
133   * Start LiveWindow mode by hooking the setters for all properties. Also calls the safeState
134   * function if one was provided.
135   */
136  public void startLiveWindowMode() {
137    if (m_safeState != null) {
138      m_safeState.run();
139    }
140    startListeners();
141  }
142
143  /**
144   * Stop LiveWindow mode by unhooking the setters for all properties. Also calls the safeState
145   * function if one was provided.
146   */
147  public void stopLiveWindowMode() {
148    stopListeners();
149    if (m_safeState != null) {
150      m_safeState.run();
151    }
152  }
153
154  /** Clear properties. */
155  @Override
156  public void clearProperties() {
157    stopListeners();
158    m_properties.clear();
159  }
160
161  /**
162   * Set the string representation of the named data type that will be used by the smart dashboard
163   * for this sendable.
164   *
165   * @param type data type
166   */
167  @Override
168  public void setSmartDashboardType(String type) {
169    m_table.getEntry(".type").setString(type);
170  }
171
172  /**
173   * Set a flag indicating if this sendable should be treated as an actuator. By default this flag
174   * is false.
175   *
176   * @param value true if actuator, false if not
177   */
178  @Override
179  public void setActuator(boolean value) {
180    m_table.getEntry(".actuator").setBoolean(value);
181    m_actuator = value;
182  }
183
184  /**
185   * Set the function that should be called to set the Sendable into a safe state. This is called
186   * when entering and exiting Live Window mode.
187   *
188   * @param func function
189   */
190  @Override
191  public void setSafeState(Runnable func) {
192    m_safeState = func;
193  }
194
195  /**
196   * Set the function that should be called to update the network table for things other than
197   * properties. Note this function is not passed the network table object; instead it should use
198   * the entry handles returned by getEntry().
199   *
200   * @param func function
201   */
202  @Override
203  public void setUpdateTable(Runnable func) {
204    m_updateTables.add(func);
205  }
206
207  /**
208   * Add a property without getters or setters. This can be used to get entry handles for the
209   * function called by setUpdateTable().
210   *
211   * @param key property name
212   * @return Network table entry
213   */
214  @Override
215  public NetworkTableEntry getEntry(String key) {
216    return m_table.getEntry(key);
217  }
218
219  /**
220   * Add a boolean property.
221   *
222   * @param key property name
223   * @param getter getter function (returns current value)
224   * @param setter setter function (sets new value)
225   */
226  @Override
227  public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter) {
228    Property property = new Property(m_table, key);
229    if (getter != null) {
230      property.m_update = entry -> entry.setBoolean(getter.getAsBoolean());
231    }
232    if (setter != null) {
233      property.m_createListener =
234          entry ->
235              entry.addListener(
236                  event -> {
237                    if (event.value.isBoolean()) {
238                      SmartDashboard.postListenerTask(
239                          () -> setter.accept(event.value.getBoolean()));
240                    }
241                  },
242                  EntryListenerFlags.kImmediate
243                      | EntryListenerFlags.kNew
244                      | EntryListenerFlags.kUpdate);
245    }
246    m_properties.add(property);
247  }
248
249  /**
250   * Add a double property.
251   *
252   * @param key property name
253   * @param getter getter function (returns current value)
254   * @param setter setter function (sets new value)
255   */
256  @Override
257  public void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter) {
258    Property property = new Property(m_table, key);
259    if (getter != null) {
260      property.m_update = entry -> entry.setDouble(getter.getAsDouble());
261    }
262    if (setter != null) {
263      property.m_createListener =
264          entry ->
265              entry.addListener(
266                  event -> {
267                    if (event.value.isDouble()) {
268                      SmartDashboard.postListenerTask(() -> setter.accept(event.value.getDouble()));
269                    }
270                  },
271                  EntryListenerFlags.kImmediate
272                      | EntryListenerFlags.kNew
273                      | EntryListenerFlags.kUpdate);
274    }
275    m_properties.add(property);
276  }
277
278  /**
279   * Add a string property.
280   *
281   * @param key property name
282   * @param getter getter function (returns current value)
283   * @param setter setter function (sets new value)
284   */
285  @Override
286  public void addStringProperty(String key, Supplier<String> getter, Consumer<String> setter) {
287    Property property = new Property(m_table, key);
288    if (getter != null) {
289      property.m_update = entry -> entry.setString(getter.get());
290    }
291    if (setter != null) {
292      property.m_createListener =
293          entry ->
294              entry.addListener(
295                  event -> {
296                    if (event.value.isString()) {
297                      SmartDashboard.postListenerTask(() -> setter.accept(event.value.getString()));
298                    }
299                  },
300                  EntryListenerFlags.kImmediate
301                      | EntryListenerFlags.kNew
302                      | EntryListenerFlags.kUpdate);
303    }
304    m_properties.add(property);
305  }
306
307  /**
308   * Add a boolean array property.
309   *
310   * @param key property name
311   * @param getter getter function (returns current value)
312   * @param setter setter function (sets new value)
313   */
314  @Override
315  public void addBooleanArrayProperty(
316      String key, Supplier<boolean[]> getter, Consumer<boolean[]> setter) {
317    Property property = new Property(m_table, key);
318    if (getter != null) {
319      property.m_update = entry -> entry.setBooleanArray(getter.get());
320    }
321    if (setter != null) {
322      property.m_createListener =
323          entry ->
324              entry.addListener(
325                  event -> {
326                    if (event.value.isBooleanArray()) {
327                      SmartDashboard.postListenerTask(
328                          () -> setter.accept(event.value.getBooleanArray()));
329                    }
330                  },
331                  EntryListenerFlags.kImmediate
332                      | EntryListenerFlags.kNew
333                      | EntryListenerFlags.kUpdate);
334    }
335    m_properties.add(property);
336  }
337
338  /**
339   * Add a double array property.
340   *
341   * @param key property name
342   * @param getter getter function (returns current value)
343   * @param setter setter function (sets new value)
344   */
345  @Override
346  public void addDoubleArrayProperty(
347      String key, Supplier<double[]> getter, Consumer<double[]> setter) {
348    Property property = new Property(m_table, key);
349    if (getter != null) {
350      property.m_update = entry -> entry.setDoubleArray(getter.get());
351    }
352    if (setter != null) {
353      property.m_createListener =
354          entry ->
355              entry.addListener(
356                  event -> {
357                    if (event.value.isDoubleArray()) {
358                      SmartDashboard.postListenerTask(
359                          () -> setter.accept(event.value.getDoubleArray()));
360                    }
361                  },
362                  EntryListenerFlags.kImmediate
363                      | EntryListenerFlags.kNew
364                      | EntryListenerFlags.kUpdate);
365    }
366    m_properties.add(property);
367  }
368
369  /**
370   * Add a string array property.
371   *
372   * @param key property name
373   * @param getter getter function (returns current value)
374   * @param setter setter function (sets new value)
375   */
376  @Override
377  public void addStringArrayProperty(
378      String key, Supplier<String[]> getter, Consumer<String[]> setter) {
379    Property property = new Property(m_table, key);
380    if (getter != null) {
381      property.m_update = entry -> entry.setStringArray(getter.get());
382    }
383    if (setter != null) {
384      property.m_createListener =
385          entry ->
386              entry.addListener(
387                  event -> {
388                    if (event.value.isStringArray()) {
389                      SmartDashboard.postListenerTask(
390                          () -> setter.accept(event.value.getStringArray()));
391                    }
392                  },
393                  EntryListenerFlags.kImmediate
394                      | EntryListenerFlags.kNew
395                      | EntryListenerFlags.kUpdate);
396    }
397    m_properties.add(property);
398  }
399
400  /**
401   * Add a raw property.
402   *
403   * @param key property name
404   * @param getter getter function (returns current value)
405   * @param setter setter function (sets new value)
406   */
407  @Override
408  public void addRawProperty(String key, Supplier<byte[]> getter, Consumer<byte[]> setter) {
409    Property property = new Property(m_table, key);
410    if (getter != null) {
411      property.m_update = entry -> entry.setRaw(getter.get());
412    }
413    if (setter != null) {
414      property.m_createListener =
415          entry ->
416              entry.addListener(
417                  event -> {
418                    if (event.value.isRaw()) {
419                      SmartDashboard.postListenerTask(() -> setter.accept(event.value.getRaw()));
420                    }
421                  },
422                  EntryListenerFlags.kImmediate
423                      | EntryListenerFlags.kNew
424                      | EntryListenerFlags.kUpdate);
425    }
426    m_properties.add(property);
427  }
428
429  /**
430   * Add a NetworkTableValue property.
431   *
432   * @param key property name
433   * @param getter getter function (returns current value)
434   * @param setter setter function (sets new value)
435   */
436  @Override
437  public void addValueProperty(
438      String key, Supplier<NetworkTableValue> getter, Consumer<NetworkTableValue> setter) {
439    Property property = new Property(m_table, key);
440    if (getter != null) {
441      property.m_update = entry -> entry.setValue(getter.get());
442    }
443    if (setter != null) {
444      property.m_createListener =
445          entry ->
446              entry.addListener(
447                  event -> {
448                    SmartDashboard.postListenerTask(() -> setter.accept(event.value));
449                  },
450                  EntryListenerFlags.kImmediate
451                      | EntryListenerFlags.kNew
452                      | EntryListenerFlags.kUpdate);
453    }
454    m_properties.add(property);
455  }
456}