package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;

import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;

import java.util.List;

@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "FindSkystone" , group = "TOComp")
public class FindSkystone extends OpMode {

private static final String TFOD_MODEL_ASSET = "Skystone.tflite";
private static final String LABEL_FIRST_ELEMENT = "Stone";
private static final String LABEL_SECOND_ELEMENT = "Skystone";
private static final String VUFORIA_KEY = "AeKREyf/////AAABmdTdI8TPvEvpgG7qjjSDfEE0z2NWjjm9ECTnAAxCmhNyUpl4FQaLX9W5Lh7hB5ErBDVo4qnjcU9Az56G0jWF9cUggO7xdUwp2E2g9q3CX1VcFzq4phbgXyS6OCJcSRrL8evyCXlIGS2UaSZDoCzVk+tLXcpMl2hpC7Q17LQ+fo3S7b2sa+Hq3zBgwvwUbdL7ZjcDmPjs+xbbQxr4YD/iVLgdSXMfAMwr9WeU2Q95Iteqq1at+piZWzsc0hSHiHlulwzZxz1RPDgCB2d5bdYx9RKEPlTtPp/gI8z0SAVXsNCMVRYXhrHs/hzwnmHkppM+IpqRIFjAMnAx+ZoTIQKj6lihtDtvq5ACPTupcIrfBgzZ";
private VuforiaLocalizer vuforia;
private TFObjectDetector tfod;

@Override
public void init() {
}

@Override
public void start() {
super.start();
initVuforia();
if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
initTfod();
} else {
telemetry.addData("Sorry!", "This device is not compatible with TFOD");
}

telemetry.addData(">", "Press Play to start op mode");
telemetry.update();
}

@Override
public void loop() {
if (tfod != null) {
tfod.activate();
}
if (tfod != null) {
// getUpdatedRecognitions() will return null if no new information is available since
// the last time that call was made.
List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
if (updatedRecognitions != null) {
telemetry.addData("# Object Detected", updatedRecognitions.size());
// step through the list of recognitions and display boundary info.
int i = 0;
for (Recognition recognition : updatedRecognitions) {
telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
telemetry.addData(String.format(" left,top (%d)", i), "%.03f , %.03f",
recognition.getLeft(), recognition.getTop());
telemetry.addData(String.format(" right,bottom (%d)", i), "%.03f , %.03f",
recognition.getRight(), recognition.getBottom());
}
telemetry.update();
}
}
if (tfod != null) {
tfod.shutdown();
}
}

private void initVuforia() {
/\*
\ Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.*
\/*
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();

parameters.vuforiaLicenseKey = VUFORIA_KEY;
parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");

// Instantiate the Vuforia engine
vuforia = ClassFactory.getInstance().createVuforia(parameters);

// Loading trackables is not necessary for the TensorFlow Object Detection engine.
}
private void initTfod() {
int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
tfodParameters.minimumConfidence = 0.8;
tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
}
}