// Path: EVLib/src/main/java/ftc/evlib/vision/processors/BeaconName.java
// public enum BeaconName {
//     WHEELS,
//     TOOLS,
//     LEGOS,
//     GEARS;
// 
//     /**
//      * Get the names of the beacons for a certain team color
//      *
//      * @param teamColor your team's color
//      * @return the list of the beacon names
//      */
//     public static List<BeaconName> getNamesForTeamColor(TeamColor teamColor) {
//         if (teamColor == TeamColor.RED) {
//             return ImmutableList.of(BeaconName.GEARS, BeaconName.TOOLS);
//         } else {
//             return ImmutableList.of(BeaconName.WHEELS, BeaconName.LEGOS);
//         }
//     }
// }
// 
// Path: EVLib/src/main/java/ftc/evlib/vision/framegrabber/GlobalFrameGrabber.java
// public static FrameGrabber frameGrabber = Fake.FRAME_GRABBER;


import android.graphics.Bitmap;
import android.support.annotation.IdRes;
import com.google.common.collect.ImmutableList;
import com.qualcomm.robotcore.util.RobotLog;
import com.vuforia.Frame;
import com.vuforia.HINT;
import com.vuforia.Image;
import com.vuforia.Matrix34F;
import com.vuforia.PIXEL_FORMAT;
import com.vuforia.State;
import com.vuforia.Tool;
import com.vuforia.Vec2F;
import com.vuforia.Vec3F;
import com.vuforia.Vuforia;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
import org.firstinspires.ftc.robotcore.internal.VuforiaLocalizerImpl;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import ftc.electronvolts.util.BasicResultReceiver;
import ftc.electronvolts.util.ResultReceiver;
import ftc.electronvolts.util.Vector2D;
import ftc.evlib.vision.processors.BeaconName;
import static ftc.evlib.vision.framegrabber.GlobalFrameGrabber.frameGrabber;
     */
    public List<Vector2D> getImageCorners(Matrix34F rawPose) {
        //top left, top right, bottom left, bottom right
        List<Vec2F> vec2fList = ImmutableList.of(
                Tool.projectPoint(getCameraCalibration(), rawPose, new Vec3F(-BEACON_TARGET_WIDTH / 2, BEACON_TARGET_HEIGHT / 2, 0)),  //top left
                Tool.projectPoint(getCameraCalibration(), rawPose, new Vec3F(BEACON_TARGET_WIDTH / 2, BEACON_TARGET_HEIGHT / 2, 0)),   //top right
                Tool.projectPoint(getCameraCalibration(), rawPose, new Vec3F(-BEACON_TARGET_WIDTH / 2, -BEACON_TARGET_HEIGHT / 2, 0)), //bottom left
                Tool.projectPoint(getCameraCalibration(), rawPose, new Vec3F(BEACON_TARGET_WIDTH / 2, -BEACON_TARGET_HEIGHT / 2, 0))   //bottom right
        );

//        Log.i(TAG, "unscaled frame size: " + new Vector2D(width, height));
//
//        Log.i(TAG, "unscaled tl: " + vef2FToVector2D(vec2fList.get(0)));
//        Log.i(TAG, "unscaled tr: " + vef2FToVector2D(vec2fList.get(1)));
//        Log.i(TAG, "unscaled bl: " + vef2FToVector2D(vec2fList.get(2)));
//        Log.i(TAG, "unscaled br: " + vef2FToVector2D(vec2fList.get(3)));

        //get average width from the top width and bottom width
        double w = ((vec2fList.get(1).getData()[1] - vec2fList.get(0).getData()[1]) + (vec2fList.get(3).getData()[1] - vec2fList.get(2).getData()[1])) / 2;
        //same for height
        double h = ((vec2fList.get(2).getData()[0] - vec2fList.get(0).getData()[0]) + (vec2fList.get(3).getData()[0] - vec2fList.get(1).getData()[0])) / 2;

//        Log.i(TAG, "beacon picture size: " + new Vector2D(w, h));

        //convert the Vec2F list to a Vector2D list and scale it to match the requested frame size
        List<Vector2D> corners = new ArrayList<>();
        for (Vec2F vec2f : vec2fList) {
            corners.add(new Vector2D(
                    (height - vec2f.getData()[1]) * widthRequest / height,
                    vec2f.getData()[0] * heightRequest / width
// Path: EVLib/src/main/java/ftc/evlib/vision/processors/BeaconName.java
// public enum BeaconName {
//     WHEELS,
//     TOOLS,
//     LEGOS,
//     GEARS;
// 
//     /**
//      * Get the names of the beacons for a certain team color
//      *
//      * @param teamColor your team's color
//      * @return the list of the beacon names
//      */
//     public static List<BeaconName> getNamesForTeamColor(TeamColor teamColor) {
//         if (teamColor == TeamColor.RED) {
//             return ImmutableList.of(BeaconName.GEARS, BeaconName.TOOLS);
//         } else {
//             return ImmutableList.of(BeaconName.WHEELS, BeaconName.LEGOS);
//         }
//     }
// }
// 
// Path: EVLib/src/main/java/ftc/evlib/vision/framegrabber/GlobalFrameGrabber.java
// public static FrameGrabber frameGrabber = Fake.FRAME_GRABBER;


// Path: EVLib/src/main/java/ftc/evlib/vision/framegrabber/VuforiaFrameGrabberInit.java
import android.graphics.Bitmap;
import android.support.annotation.IdRes;
import com.google.common.collect.ImmutableList;
import com.qualcomm.robotcore.util.RobotLog;
import com.vuforia.Frame;
import com.vuforia.HINT;
import com.vuforia.Image;
import com.vuforia.Matrix34F;
import com.vuforia.PIXEL_FORMAT;
import com.vuforia.State;
import com.vuforia.Tool;
import com.vuforia.Vec2F;
import com.vuforia.Vec3F;
import com.vuforia.Vuforia;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
import org.firstinspires.ftc.robotcore.internal.VuforiaLocalizerImpl;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import ftc.electronvolts.util.BasicResultReceiver;
import ftc.electronvolts.util.ResultReceiver;
import ftc.electronvolts.util.Vector2D;
import ftc.evlib.vision.processors.BeaconName;
import static ftc.evlib.vision.framegrabber.GlobalFrameGrabber.frameGrabber;

     */
    public List<Vector2D> getImageCorners(Matrix34F rawPose) {
        //top left, top right, bottom left, bottom right
        List<Vec2F> vec2fList = ImmutableList.of(
                Tool.projectPoint(getCameraCalibration(), rawPose, new Vec3F(-BEACON_TARGET_WIDTH / 2, BEACON_TARGET_HEIGHT / 2, 0)),  //top left
                Tool.projectPoint(getCameraCalibration(), rawPose, new Vec3F(BEACON_TARGET_WIDTH / 2, BEACON_TARGET_HEIGHT / 2, 0)),   //top right
                Tool.projectPoint(getCameraCalibration(), rawPose, new Vec3F(-BEACON_TARGET_WIDTH / 2, -BEACON_TARGET_HEIGHT / 2, 0)), //bottom left
                Tool.projectPoint(getCameraCalibration(), rawPose, new Vec3F(BEACON_TARGET_WIDTH / 2, -BEACON_TARGET_HEIGHT / 2, 0))   //bottom right
        );

//        Log.i(TAG, "unscaled frame size: " + new Vector2D(width, height));
//
//        Log.i(TAG, "unscaled tl: " + vef2FToVector2D(vec2fList.get(0)));
//        Log.i(TAG, "unscaled tr: " + vef2FToVector2D(vec2fList.get(1)));
//        Log.i(TAG, "unscaled bl: " + vef2FToVector2D(vec2fList.get(2)));
//        Log.i(TAG, "unscaled br: " + vef2FToVector2D(vec2fList.get(3)));

        //get average width from the top width and bottom width
        double w = ((vec2fList.get(1).getData()[1] - vec2fList.get(0).getData()[1]) + (vec2fList.get(3).getData()[1] - vec2fList.get(2).getData()[1])) / 2;
        //same for height
        double h = ((vec2fList.get(2).getData()[0] - vec2fList.get(0).getData()[0]) + (vec2fList.get(3).getData()[0] - vec2fList.get(1).getData()[0])) / 2;

//        Log.i(TAG, "beacon picture size: " + new Vector2D(w, h));

        //convert the Vec2F list to a Vector2D list and scale it to match the requested frame size
        List<Vector2D> corners = new ArrayList<>();
        for (Vec2F vec2f : vec2fList) {
            corners.add(new Vector2D(
                    (height - vec2f.getData()[1]) * widthRequest / height,
                    vec2f.getData()[0] * heightRequest / width
            ));