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;
019
020import edu.wpi.first.util.protobuf.ProtobufSerializable;
021import java.util.List;
022import org.photonvision.common.dataflow.structures.PacketSerde;
023import org.photonvision.struct.MultiTargetPNPResultSerde;
024import org.photonvision.targeting.proto.MultiTargetPNPResultProto;
025import org.photonvision.targeting.serde.PhotonStructSerializable;
026
027public class MultiTargetPNPResult
028        implements ProtobufSerializable, PhotonStructSerializable<MultiTargetPNPResult> {
029    // Seeing 32 apriltags at once seems like a sane limit
030    private static final int MAX_IDS = 32;
031
032    public PnpResult estimatedPose = new PnpResult();
033
034    /** The fiducial IDs used to calculate this multi-target result. */
035    public List<Short> fiducialIDsUsed = List.of();
036
037    /** Used for serialization and tests. */
038    public MultiTargetPNPResult() {}
039
040    public MultiTargetPNPResult(PnpResult results, List<Short> ids) {
041        estimatedPose = results;
042        fiducialIDsUsed = ids;
043    }
044
045    @Override
046    public int hashCode() {
047        final int prime = 31;
048        int result = 1;
049        result = prime * result + ((estimatedPose == null) ? 0 : estimatedPose.hashCode());
050        result = prime * result + ((fiducialIDsUsed == null) ? 0 : fiducialIDsUsed.hashCode());
051        return result;
052    }
053
054    @Override
055    public boolean equals(Object obj) {
056        if (this == obj) return true;
057        if (obj == null) return false;
058        if (getClass() != obj.getClass()) return false;
059        MultiTargetPNPResult other = (MultiTargetPNPResult) obj;
060        if (estimatedPose == null) {
061            if (other.estimatedPose != null) return false;
062        } else if (!estimatedPose.equals(other.estimatedPose)) return false;
063        if (fiducialIDsUsed == null) {
064            if (other.fiducialIDsUsed != null) return false;
065        } else if (!fiducialIDsUsed.equals(other.fiducialIDsUsed)) return false;
066        return true;
067    }
068
069    @Override
070    public String toString() {
071        return "MultiTargetPNPResult [estimatedPose="
072                + estimatedPose
073                + ", fiducialIDsUsed="
074                + fiducialIDsUsed
075                + "]";
076    }
077
078    /** MultiTargetPNPResult protobuf for serialization. */
079    public static final MultiTargetPNPResultProto proto = new MultiTargetPNPResultProto();
080
081    /** MultiTargetPNPResult PhotonStruct for serialization. */
082    public static final MultiTargetPNPResultSerde photonStruct = new MultiTargetPNPResultSerde();
083
084    @Override
085    public PacketSerde<MultiTargetPNPResult> getSerde() {
086        return photonStruct;
087    }
088}