001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.common.networktables;
019
020import edu.wpi.first.networktables.RawSubscriber;
021import java.util.ArrayList;
022import java.util.List;
023import org.photonvision.common.dataflow.structures.Packet;
024import org.photonvision.common.dataflow.structures.PacketSerde;
025
026public class PacketSubscriber<T> implements AutoCloseable {
027    public static class PacketResult<U> {
028        public final U value;
029        public final long timestamp;
030
031        public PacketResult(U value, long timestamp) {
032            this.value = value;
033            this.timestamp = timestamp;
034        }
035
036        public PacketResult() {
037            this(null, 0);
038        }
039    }
040
041    public final RawSubscriber subscriber;
042    private final PacketSerde<T> serde;
043
044    private final Packet packet = new Packet(1);
045
046    /**
047     * Create a PacketSubscriber
048     *
049     * @param subscriber NT subscriber. Set pollStorage to 1 to make get() faster
050     * @param serde How we convert raw to actual things
051     */
052    public PacketSubscriber(RawSubscriber subscriber, PacketSerde<T> serde) {
053        this.subscriber = subscriber;
054        this.serde = serde;
055    }
056
057    /** Parse one chunk of timestamped data into T */
058    private PacketResult<T> parse(byte[] data, long timestamp) {
059        packet.clear();
060        packet.setData(data);
061        if (packet.getSize() < 1) {
062            return new PacketResult<T>();
063        }
064
065        return new PacketResult<>(serde.unpack(packet), timestamp);
066    }
067
068    /**
069     * Get the latest value sent over NT. If the value has never been set, returns the provided
070     * default
071     */
072    public PacketResult<T> get() {
073        // Get /all/ changes since last call to readQueue
074        var data = subscriber.getAtomic();
075
076        // Topic has never been published to?
077        if (data.timestamp == 0) {
078            return new PacketResult<>();
079        }
080
081        return parse(data.value, data.timestamp);
082    }
083
084    @Override
085    public void close() {
086        subscriber.close();
087    }
088
089    // TODO - i can see an argument for moving this logic all here instead of keeping in photoncamera
090    public String getInterfaceUUID() {
091        // ntcore hands us a JSON string with leading/trailing quotes - remove those
092        var uuidStr = subscriber.getTopic().getProperty("message_uuid");
093
094        // "null" can be returned if the property does not exist. From system knowledge, uuid can never
095        // be the string literal "null".
096        if (uuidStr.equals("null")) {
097            return "";
098        }
099
100        return uuidStr.replace("\"", "");
101    }
102
103    public List<PacketResult<T>> getAllChanges() {
104        List<PacketResult<T>> ret = new ArrayList<>();
105
106        // Get /all/ changes since last call to readQueue
107        var changes = subscriber.readQueue();
108
109        for (var change : changes) {
110            ret.add(parse(change.value, change.timestamp));
111        }
112
113        return ret;
114    }
115}