power-play/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/AprilTagAutonBase.java

package org.firstinspires.ftc.teamcode.auto;

import android.annotation.SuppressLint;

import com.acmerobotics.dashboard.FtcDashboard;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.config.RuntimeConfig;
import org.openftc.apriltag.AprilTagDetection;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;

import java.util.ArrayList;

public abstract class AprilTagAutonBase extends LinearOpMode {
    OpenCvCamera camera;
    AprilTagDetectionPipeline aprilTagDetectionPipeline;

    // Lens intrinsics
    // UNITS ARE PIXELS
    // NOTE: this calibration is for the C920 webcam at 800x448.
    // You will need to do your own calibration for other configurations!
    double fx = 578.272;
    double fy = 578.272;
    double cx = 402.145;
    double cy = 221.506;

    // UNITS ARE METERS
    double tagSize = 0.0381; // 1.5 in

    // All from the 36h11 family
    int[][] ID_TAGS_OF_INTEREST = {
            {
                    584,
                    449,
                    275,
            },
            {
                    125,
                    345,
                    392,
            },
            {
                    87,
                    93,
                    554
            },
            {
                    1,
                    2,
                    3
            }
    };

    int VARIANT_COUNT = ID_TAGS_OF_INTEREST.length;

    AprilTagDetection[][] tagsOfInterest = new AprilTagDetection[VARIANT_COUNT][3];

    abstract void onInit();
    abstract void onPlay(int signalZone);

    @Override
    public void runOpMode() {
        if (RuntimeConfig.SIGNAL_SLEEVE_VARIANT == null) {
            double endTime = getRuntime() + 5;
            while (!isStarted() && getRuntime() < endTime) {
                telemetry.addLine("Signal sleeve variant not set!");
                telemetry.addLine("Press play or wait 5 seconds to end the opmode.");
                telemetry.addLine("Here is a flashing block to get your attention:");
                telemetry.addLine("####################################");
                telemetry.addLine("####################################");
                telemetry.addLine("####################################");
                telemetry.update();

                telemetry.addLine("Signal sleeve variant not set!");
                telemetry.addLine("Press play or wait 5 seconds to end the opmode.");
                telemetry.addLine("Here is a flashing block to get your attention:");
                telemetry.addLine("");
                telemetry.addLine("");
                telemetry.addLine("");
                telemetry.update();
            }

            return;
        }

        onInit();

        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
        camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
        aprilTagDetectionPipeline = new AprilTagDetectionPipeline(tagSize, fx, fy, cx, cy);

        camera.setPipeline(aprilTagDetectionPipeline);
        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
            @Override
            public void onOpened() {
                camera.startStreaming(800,448, OpenCvCameraRotation.SIDEWAYS_RIGHT);
                FtcDashboard dashboard = FtcDashboard.getInstance();
                dashboard.startCameraStream(camera, 0);
            }

            @Override
            public void onError(int errorCode) {}
        });

        telemetry.setMsTransmissionInterval(50);

        Integer[] mostRecentlySeenPerVariant = new Integer[VARIANT_COUNT];

        /*
         * The INIT-loop:
         * This REPLACES waitForStart!
         */
        while (!isStarted() && !isStopRequested()) {
            telemetry.addLine("Chosen variant: " + RuntimeConfig.signalSleeveVariantName());

            ArrayList<AprilTagDetection> currentDetections = aprilTagDetectionPipeline.getLatestDetections();

            ArrayList<Integer>[] tagsFound = new ArrayList[VARIANT_COUNT];
            // fill the array, cause i dont know how to do this better
            for (int i = 0; i < tagsFound.length; i++) tagsFound[i] = new ArrayList<>();

            for (AprilTagDetection tag : currentDetections) {
                for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
                    for (int id_index = 0; id_index < 3; id_index++) {
                        if (tag.id == ID_TAGS_OF_INTEREST[variant_index][id_index]) {
                            tagsFound[variant_index].add(id_index);

                            tagsOfInterest[variant_index][id_index] = tag;
                        }
                    }
                }
            }

            for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
                if (tagsFound[variant_index].size() == 1) {
                    mostRecentlySeenPerVariant[variant_index] = tagsFound[variant_index].get(0);
                }
            }

            if (tagsFound[RuntimeConfig.SIGNAL_SLEEVE_VARIANT].size() == 1) {
                int id = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];

                telemetry.addLine("Tag " + RuntimeConfig.signalSleeveVariantName() + "-" + (id + 1) + " is in sight!\n\nLocation data:");
                tagToTelemetry(tagsOfInterest[RuntimeConfig.SIGNAL_SLEEVE_VARIANT][id]);
            } else {
                telemetry.addLine("Don't currently see any tags from variant " + RuntimeConfig.signalSleeveVariantName() + " :(");

                if (mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT] == null) {
                    telemetry.addLine("(No tags have ever been seen)");
                } else {
                    int id = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];

                    telemetry.addLine("\nBut we HAVE seen tags before; tag " + RuntimeConfig.signalSleeveVariantName() + "-" + (id + 1) + " last seen at:");
                    tagToTelemetry(tagsOfInterest[RuntimeConfig.SIGNAL_SLEEVE_VARIANT][id]);
                }
            }

            telemetry.update();
            sleep(20);
        }

        /*
         * The START command just came in: now work off the latest snapshot acquired
         * during the init loop.
         */

        int signalZone;

        if (mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT] == null) {
            telemetry.addLine("Uh oh, we never saw variant " + RuntimeConfig.signalSleeveVariantName() + "! Checking other variants...");

            Integer id = null;
            Integer variant = null;

            for (int variant_index = 0; variant_index < VARIANT_COUNT; variant_index++) {
                if (mostRecentlySeenPerVariant[variant_index] != null) {
                    id = mostRecentlySeenPerVariant[variant_index];
                    variant = variant_index;
                }
            }

            if (id != null) {
                telemetry.addLine("We saw tag " + RuntimeConfig.signalSleeveVariantName(variant) + "-" + (id + 1) + " at some point, using that.");
                signalZone = id;
            } else {
                telemetry.addLine("Nope, nothing, nada, defaulting to signal zone 1.");
                signalZone = 1;
            }
            telemetry.update();
        } else {
            signalZone = mostRecentlySeenPerVariant[RuntimeConfig.SIGNAL_SLEEVE_VARIANT];
        }

        this.onPlay(signalZone + 1);
    }

    @SuppressLint("DefaultLocale")
    void tagToTelemetry(AprilTagDetection detection) {
        telemetry.addLine(String.format("\nDetected tag ID=%d", detection.id));
        telemetry.addLine(String.format("Translation X: %.2f metres", detection.pose.x));
        telemetry.addLine(String.format("Translation Y: %.2f metres", detection.pose.y));
        telemetry.addLine(String.format("Translation Z: %.2f metres", detection.pose.z));
        telemetry.addLine(String.format("Rotation Yaw: %.2f degrees", Math.toDegrees(detection.pose.yaw)));
        telemetry.addLine(String.format("Rotation Pitch: %.2f degrees", Math.toDegrees(detection.pose.pitch)));
        telemetry.addLine(String.format("Rotation Roll: %.2f degrees", Math.toDegrees(detection.pose.roll)));
    }
}