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.targeting.proto;
019
020import edu.wpi.first.util.protobuf.Protobuf;
021import java.util.Optional;
022import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult;
023import org.photonvision.targeting.MultiTargetPNPResult;
024import org.photonvision.targeting.PhotonPipelineResult;
025import org.photonvision.targeting.PhotonTrackedTarget;
026import us.hebi.quickbuf.Descriptors.Descriptor;
027
028public class PhotonPipelineResultProto
029        implements Protobuf<PhotonPipelineResult, ProtobufPhotonPipelineResult> {
030    @Override
031    public Class<PhotonPipelineResult> getTypeClass() {
032        return PhotonPipelineResult.class;
033    }
034
035    @Override
036    public Descriptor getDescriptor() {
037        return ProtobufPhotonPipelineResult.getDescriptor();
038    }
039
040    @Override
041    public ProtobufPhotonPipelineResult createMessage() {
042        return ProtobufPhotonPipelineResult.newInstance();
043    }
044
045    @Override
046    public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) {
047        return new PhotonPipelineResult(
048                msg.getSequenceId(),
049                msg.getCaptureTimestampMicros(),
050                msg.getNtPublishTimestampMicros(),
051                msg.getTimeSinceLastPongMicros(),
052                PhotonTrackedTarget.proto.unpack(msg.getTargets()),
053                msg.hasMultiTargetResult()
054                        ? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()))
055                        : Optional.empty());
056    }
057
058    @Override
059    public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) {
060        PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets());
061
062        if (value.getMultiTagResult().isPresent()) {
063            MultiTargetPNPResult.proto.pack(
064                    msg.getMutableMultiTargetResult(), value.getMultiTagResult().get());
065        } else {
066            msg.clearMultiTargetResult();
067        }
068
069        msg.setSequenceId(value.metadata.getSequenceID());
070        msg.setCaptureTimestampMicros(value.metadata.getCaptureTimestampMicros());
071        msg.setNtPublishTimestampMicros(value.metadata.getPublishTimestampMicros());
072        msg.setTimeSinceLastPongMicros(value.metadata.timeSinceLastPong);
073    }
074}