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
026@SuppressWarnings("doclint")
027public class PacketSubscriber<T> implements AutoCloseable {
028    public static class PacketResult<U> {
029        public final U value;
030        public final long timestamp;
031
032        public PacketResult(U value, long timestamp) {
033            this.value = value;
034            this.timestamp = timestamp;
035        }
036
037        public PacketResult() {
038            this(null, 0);
039        }
040    }
041
042    public final RawSubscriber subscriber;
043    private final PacketSerde<T> serde;
044
045    private final Packet packet = new Packet(1);
046
047    /**
048     * Create a PacketSubscriber
049     *
050     * @param subscriber NT subscriber. Set pollStorage to 1 to make get() faster
051     * @param serde How we convert raw to actual things
052     */
053    public PacketSubscriber(RawSubscriber subscriber, PacketSerde<T> serde) {
054        this.subscriber = subscriber;
055        this.serde = serde;
056    }
057
058    /** Parse one chunk of timestamped data into T */
059    private PacketResult<T> parse(byte[] data, long timestamp) {
060        packet.clear();
061        packet.setData(data);
062        if (packet.getSize() < 1) {
063            return new PacketResult<T>();
064        }
065
066        return new PacketResult<>(serde.unpack(packet), timestamp);
067    }
068
069    /**
070     * Get the latest value sent over NT. If the value has never been set, returns the provided
071     * default
072     */
073    public PacketResult<T> get() {
074        // Get /all/ changes since last call to readQueue
075        var data = subscriber.getAtomic();
076
077        // Topic has never been published to?
078        if (data.timestamp == 0) {
079            return new PacketResult<>();
080        }
081
082        return parse(data.value, data.timestamp);
083    }
084
085    @Override
086    public void close() {
087        subscriber.close();
088    }
089
090    // TODO - i can see an argument for moving this logic all here instead of keeping in photoncamera
091    public String getInterfaceUUID() {
092        // ntcore hands us a JSON string with leading/trailing quotes - remove those
093        var uuidStr = subscriber.getTopic().getProperty("message_uuid");
094
095        // "null" can be returned if the property does not exist. From system knowledge, uuid can never
096        // be the string literal "null".
097        if (uuidStr.equals("null")) {
098            return "";
099        }
100
101        return uuidStr.replace("\"", "");
102    }
103
104    public List<PacketResult<T>> getAllChanges() {
105        // Get /all/ changes since last call to readQueue
106        var changes = subscriber.readQueue();
107
108        List<PacketResult<T>> ret = new ArrayList<>(changes.length);
109        for (var change : changes) {
110            ret.add(parse(change.value, change.timestamp));
111        }
112
113        return ret;
114    }
115}