import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
/**
 * This method sets the properties of the specified target.
 *
 * @param index specifies the target index in the XML file.
 * @param name specifies the target name.
 * @param locationOnField specifies the target location on the field, can be null if no robot tracking.
 * @param phoneLocationOnRobot specifies the phone location on the robot, can be null if no robot tracking.
 */
public void setTargetInfo(int index, String name, OpenGLMatrix locationOnField, OpenGLMatrix phoneLocationOnRobot)
{
    if (targetList != null)
    {
        VuforiaTrackable target = targetList.get(index);
        target.setName(name);

        if (locationOnField != null)
        {
            target.setLocation(locationOnField);
        }

        if (phoneLocationOnRobot != null)
        {
            ((VuforiaTrackableDefaultListener) target.getListener()).setPhoneInformation(
                    phoneLocationOnRobot, cameraDir);
        }
    }
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public void setupVuforia()
{

	parameters = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); //remove parameters to hide phone tracking
	parameters.vuforiaLicenseKey = VUFORIA_KEY;
	parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
	parameters.useExtendedTracking = false; //extended tracking is quite inaccurate
	vuforiaLocalizer = ClassFactory.createVuforiaLocalizer(parameters);
	
	visionTargets = vuforiaLocalizer.loadTrackablesFromAsset("RelicVuMark");

	relicVuMark = visionTargets.get(0);
	relicVuMark.setName("RelicVuMark");
	relicVuMark.setLocation(createMatrix(0,0,0,0,0,0));
	
	phoneLocation = createMatrix(0,0,0,0,0,0);
	
	listener = (VuforiaTrackableDefaultListener) relicVuMark.getListener();
	listener.setPhoneInformation(phoneLocation, parameters.cameraDirection);
	
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public void setupVuforia()
{
    parameters = new VuforiaLocalizer.Parameters(); //remove parameters to hide phone tracking
    parameters.vuforiaLicenseKey = VUFORIA_KEY;
    parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
    parameters.useExtendedTracking = true; //extended tracking is quite inaccurate
    vuforiaLocalizer = ClassFactory.createVuforiaLocalizer(parameters);

    visionTargets = vuforiaLocalizer.loadTrackablesFromAsset("RelicVuMark");

    relicVuMark = visionTargets.get(0);
    relicVuMark.setName("RelicVuMark");
    //relicVuMark.setLocation(createMatrix(0,0,0,0,0,0));

    //phoneLocation = createMatrix(0,0,0,0,0,0);

    listener = (VuforiaTrackableDefaultListener) relicVuMark.getListener();
    //listener.setPhoneInformation(phoneLocation, parameters.cameraDirection);
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public void updateLocation(){
    OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicVuMark.getListener()).getPose();
    telemetry.addData("Pose", format(pose));

    /* We further illustrate how to decompose the pose into useful rotational and
    * translational components */
    if (pose != null) {
        VectorF trans = pose.getTranslation();
        Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);

        // Extract the X, Y, and Z components of the offset of the target relative to the robot
        tX = trans.get(0);
        tY = trans.get(1);
        tZ = trans.get(2);

        // Extract the rotational components of the target relative to the robot
        rX = rot.firstAngle;
        rY = rot.secondAngle;
        rZ = rot.thirdAngle;
        //Z is forward-backward
        //x is sideways
    }
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public void setupVuforia()
{
    parameters = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); //remove parameters to hide phone tracking
    parameters.vuforiaLicenseKey = VUFORIA_KEY;
    parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
    parameters.useExtendedTracking = true; //extended tracking can be inaccurate

    vuforiaLocalizer = ClassFactory.createVuforiaLocalizer(parameters);
    visionTargets = vuforiaLocalizer.loadTrackablesFromAsset("RelicVuMark");

    relicVuMark = visionTargets.get(0);
    relicVuMark.setName("RelicRecovery");
    relicVuMark.setLocation(createMatrix(0,0,0,0,0,0));

    phoneLocation = createMatrix(0,0,0,0,0,0);

    listener = (VuforiaTrackableDefaultListener) relicVuMark.getListener();
    listener.setPhoneInformation(phoneLocation, parameters.cameraDirection);
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public void setupVuforia()
{

    parameters = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); //remove parameters to hide phone tracking
    parameters.vuforiaLicenseKey = VUFORIA_KEY;
    parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
    parameters.useExtendedTracking = false; //extended tracking is quite inaccurate
    vuforiaLocalizer = ClassFactory.createVuforiaLocalizer(parameters);

    visionTargets = vuforiaLocalizer.loadTrackablesFromAsset("RelicVuMark");

    relicVuMark = visionTargets.get(0);
    relicVuMark.setName("RelicVuMark");
    relicVuMark.setLocation(createMatrix(0,0,0,0,0,0));

    phoneLocation = createMatrix(0,0,0,0,0,0);

    listener = (VuforiaTrackableDefaultListener) relicVuMark.getListener();
    listener.setPhoneInformation(phoneLocation, parameters.cameraDirection);

}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public static int waitForBeaconDetection(LinearOpMode opMode, Image img,
                                   VuforiaTrackableDefaultListener beacon,
                                   CameraCalibration camCal, long timeOut) {
    int config = NOT_VISIBLE;
    long beginTime = System.currentTimeMillis();
    while (config == NOT_VISIBLE && System.currentTimeMillis() - beginTime < timeOut && opMode.opModeIsActive()) {
        config = getBeaconConfig(img, beacon, camCal);
        opMode.idle();
    }

    return config;
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public VectorF anglesFromTarget(VuforiaTrackableDefaultListener image){
    float [] data = image.getRawPose().getData();
    float [] [] rotation = {{data[0], data[1]}, {data[4], data[5], data[6]}, {data[8], data[9], data[10]}};
    double thetaX = Math.atan2(rotation[2][1], rotation[2][2]);
    double thetaY = Math.atan2(-rotation[2][0], Math.sqrt(rotation[2][1] * rotation[2][1] + rotation[2][2] * rotation[2][2]));
    double thetaZ = Math.atan2(rotation[1][0], rotation[0][0]);
    return new VectorF((float)thetaX, (float)thetaY, (float)thetaZ);
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public static int getBeaconConfig(Image img, VuforiaTrackableDefaultListener beacon, CameraCalibration camCal) {

        OpenGLMatrix pose = beacon.getRawPose();

        if (pose != null && img != null && img.getPixels() != null) {

            Matrix34F rawPose = new Matrix34F();
            float[] poseData = Arrays.copyOfRange(pose.transposed().getData(), 0, 12);

            rawPose.setData(poseData);

            float[][] corners = new float[4][2];

            corners[0] = Tool.projectPoint(camCal, rawPose, new Vec3F(-127, 276, 0)).getData(); //upper left of beacon
            corners[1] = Tool.projectPoint(camCal, rawPose, new Vec3F(127, 276, 0)).getData(); //upper right of beacon
            corners[2] = Tool.projectPoint(camCal, rawPose, new Vec3F(127, -92, 0)).getData(); //lower right of beacon
            corners[3] = Tool.projectPoint(camCal, rawPose, new Vec3F(-127, -92, 0)).getData(); //lower left of beacon

            //getting camera image...
            Bitmap bm = Bitmap.createBitmap(img.getWidth(), img.getHeight(), Bitmap.Config.RGB_565);
            bm.copyPixelsFromBuffer(img.getPixels());

            //turning the corner pixel coordinates into a proper bounding box
            Mat crop = bitmapToMat(bm, CvType.CV_8UC3);
            float x = Math.min(Math.min(corners[1][0], corners[3][0]), Math.min(corners[0][0], corners[2][0]));
            float y = Math.min(Math.min(corners[1][1], corners[3][1]), Math.min(corners[0][1], corners[2][1]));
            float width = Math.max(Math.abs(corners[0][0] - corners[2][0]), Math.abs(corners[1][0] - corners[3][0]));
            float height = Math.max(Math.abs(corners[0][1] - corners[2][1]), Math.abs(corners[1][1] - corners[3][1]));


            //make sure our bounding box doesn't go outside of the image
            //OpenCV doesn't like that...
            x = Math.max(x, 0);
            y = Math.max(y, 0);
            width = (x + width > crop.cols())? crop.cols() - x : width;
            height = (y + height > crop.rows())? crop.rows() - y : height;

            //cropping bounding box out of camera image
            final Mat cropped = new Mat(crop, new Rect((int) x, (int) y, (int) width, (int) height));

            //filtering out non-beacon-blue colours in HSV colour space
            Imgproc.cvtColor(cropped, cropped, Imgproc.COLOR_RGB2HSV_FULL);

            //get filtered mask
            //if pixel is within acceptable blue-beacon-colour range, it's changed to white.
            //Otherwise, it's turned to black
            Mat mask = new Mat();

            Core.inRange(cropped, BEACON_BLUE_LOW, BEACON_BLUE_HIGH, mask);
            Moments mmnts = Imgproc.moments(mask, true);

            //calculating centroid of the resulting binary mask via image moments
            Log.i("CentroidX", "" + ((mmnts.get_m10() / mmnts.get_m00())));
            Log.i("CentroidY", "" + ((mmnts.get_m01() / mmnts.get_m00())));

            //checking if blue either takes up the majority of the image (which means the beacon is all blue)
            //or if there's barely any blue in the image (which means the beacon is all red or off)
//            if (mmnts.get_m00() / mask.total() > 0.8) {
//                return VortexUtils.BEACON_ALL_BLUE;
//            } else if (mmnts.get_m00() / mask.total() < 0.1) {
//                return VortexUtils.BEACON_NO_BLUE;
//            }//elseif

            //Note: for some reason, we end up with a image that is rotated 90 degrees
            //if centroid is in the bottom half of the image, the blue beacon is on the left
            //if the centroid is in the top half, the blue beacon is on the right
            if ((mmnts.get_m01() / mmnts.get_m00()) < cropped.rows() / 2) {
                return BEACON_RED_BLUE;
            } else {
                return BEACON_BLUE_RED;
            }
        }

        return NOT_VISIBLE;
    }
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
/**
 * This method determines if the target is visible.
 *
 * @param target specifies the target object.
 * @return true if the target is in view, false otherwise.
 */
public boolean isTargetVisible(VuforiaTrackable target)
{
    VuforiaTrackableDefaultListener listener = (VuforiaTrackableDefaultListener)target.getListener();
    return listener.isVisible();
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
/**
 * This method returns the position matrix of the specified target.
 *
 * @param target specifies the target to get the position matrix.
 * @return position matrix of the specified target.
 */
public OpenGLMatrix getTargetPose(VuforiaTrackable target)
{
    VuforiaTrackableDefaultListener listener = (VuforiaTrackableDefaultListener)target.getListener();
    return listener.getPose();
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
/**
 * This method determines the robot location by the given target.
 *
 * @param target specifies the target to be used to determine robot location.
 * @return robot location matrix.
 */
public OpenGLMatrix getRobotLocation(VuforiaTrackable target)
{
    VuforiaTrackableDefaultListener listener = (VuforiaTrackableDefaultListener)target.getListener();
    return listener.getRobotLocation();
}
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public void updateLocation(){

        OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicVuMark.getListener()).getPose();

        telemetry.addData("Pose", format(pose));



                    /* We further illustrate how to decompose the pose into useful rotational and

                 * translational components */

        if (pose != null) {

            VectorF trans = pose.getTranslation();

            Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);



            // Extract the X, Y, and Z components of the offset of the target relative to the robot

            tX = trans.get(0);

            tY = trans.get(1);

            tZ = trans.get(2);



            // Extract the rotational components of the target relative to the robot

            rX = rot.firstAngle;

            rY = rot.secondAngle;

            rZ = rot.thirdAngle;

            // Z is forward-backward
            //x is sideways
        }

    }
 

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; //导入依赖的package包/类
public void setupVuforia()

    {



        parameters = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); //remove parameters to hide phone tracking

        parameters.vuforiaLicenseKey = VUFORIA_KEY;

        parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;

        parameters.useExtendedTracking = true; //extended tracking can be inaccurate

        vuforiaLocalizer = ClassFactory.createVuforiaLocalizer(parameters);



        visionTargets = vuforiaLocalizer.loadTrackablesFromAsset("RelicVuMark");



        relicVuMark = visionTargets.get(0);

        relicVuMark.setName("RelicRecovery");

        relicVuMark.setLocation(createMatrix(0,0,0,0,0,0));



        phoneLocation = createMatrix(0,0,0,0,0,0);



        listener = (VuforiaTrackableDefaultListener) relicVuMark.getListener();

        listener.setPhoneInformation(phoneLocation, parameters.cameraDirection);



    }