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    public List<Short> fiducialIDsUsed = List.of();
034
035    public MultiTargetPNPResult() {}
036
037    public MultiTargetPNPResult(PnpResult results, List<Short> ids) {
038        estimatedPose = results;
039        fiducialIDsUsed = ids;
040    }
041
042    @Override
043    public int hashCode() {
044        final int prime = 31;
045        int result = 1;
046        result = prime * result + ((estimatedPose == null) ? 0 : estimatedPose.hashCode());
047        result = prime * result + ((fiducialIDsUsed == null) ? 0 : fiducialIDsUsed.hashCode());
048        return result;
049    }
050
051    @Override
052    public boolean equals(Object obj) {
053        if (this == obj) return true;
054        if (obj == null) return false;
055        if (getClass() != obj.getClass()) return false;
056        MultiTargetPNPResult other = (MultiTargetPNPResult) obj;
057        if (estimatedPose == null) {
058            if (other.estimatedPose != null) return false;
059        } else if (!estimatedPose.equals(other.estimatedPose)) return false;
060        if (fiducialIDsUsed == null) {
061            if (other.fiducialIDsUsed != null) return false;
062        } else if (!fiducialIDsUsed.equals(other.fiducialIDsUsed)) return false;
063        return true;
064    }
065
066    @Override
067    public String toString() {
068        return "MultiTargetPNPResult [estimatedPose="
069                + estimatedPose
070                + ", fiducialIDsUsed="
071                + fiducialIDsUsed
072                + "]";
073    }
074
075    public static final MultiTargetPNPResultProto proto = new MultiTargetPNPResultProto();
076
077    // tODO!
078    public static final MultiTargetPNPResultSerde photonStruct = new MultiTargetPNPResultSerde();
079
080    @Override
081    public PacketSerde<MultiTargetPNPResult> getSerde() {
082        return photonStruct;
083    }
084}