package com.edinarobotics.zed.commands;

import com.edinarobotics.utils.math.Point2;
import com.edinarobotics.utils.math.VariablePoint2;

public class FixedPointVisionTrackingCommand extends VisionTrackingCommand {
    public static final double PYRAMID_BACK_MIDDLE_GOAL_WIDTH = 0.632219;
    public static final double PYRAMID_BACK_MIDDLE_GOAL_HEIGHT = 0.142857;
    
    private Point2 fixedPoint;
    
    public static final Point2 PYRAMID_BACK_MIDDLE_NEW = new Point2(-0.018, -0.25);
    public static final Point2 PYRAMID_BACK_RIGHT = new Point2(-0.063829, -0.226891);
    public static final Point2 PYRAMID_BACK_MIDDLE = new Point2(0.0273556, -0.29411);
    public static final Point2 PYRAMID_BACK_MIDDLE_NEW2 = new Point2(-0.143, -0.241);
    public static final Point2 PYRAMID_BACK_MIDDLE_TUNNEL = new Point2(PYRAMID_BACK_MIDDLE_NEW.getX() + (1.3 * PYRAMID_BACK_MIDDLE_GOAL_WIDTH),
            PYRAMID_BACK_MIDDLE_NEW.getY() - (1.2 * PYRAMID_BACK_MIDDLE_GOAL_HEIGHT));
    //public static final Point2 PYRAMID_BACK_MIDDLE_AUTO = new Point2(PYRAMID_BACK_MIDDLE_NEW.getX() - (0.5 * PYRAMID_BACK_MIDDLE_GOAL_WIDTH),
    //        PYRAMID_BACK_MIDDLE_NEW.getY() - (0.87 * PYRAMID_BACK_MIDDLE_GOAL_HEIGHT));
    
    /*
     * TO RESET VT POINT: -0.5 goal width and -0.5 goal height
     */
    
    public FixedPointVisionTrackingCommand(double xSetpoint, double ySetpoint) {
        super();
        fixedPoint = new Point2(xSetpoint, ySetpoint);
    }
    
    public FixedPointVisionTrackingCommand(double xSetpoint, double ySetpoint, byte goalType) {
        super(goalType);
        fixedPoint = new Point2(xSetpoint, ySetpoint);
    }
    
    public FixedPointVisionTrackingCommand(Point2 point2) {
        super();
        fixedPoint = point2;
    }
    
    public FixedPointVisionTrackingCommand(Point2 point2, byte goalType) {
        super(goalType);
        fixedPoint = point2;
    }
    
    protected double getXSetpoint(double distance) {
        return fixedPoint.getX();
    }
    
    protected double getYSetpoint(double distance) {
        return fixedPoint.getY();
    }
}

@@ -107,33 +107,132 @@
 ics.
-utils.math.VariablePoint2
+zed.vision.Target;%0Aimport com.edinarobotics.zed.vision.TargetType;%0Aimport com.edinarobotics.zed.vision.VisionTrackingCommand
 ;%0A%0Ap
@@ -492,16 +492,51 @@
 dPoint;%0A
+    private TargetType targetType;%0A
     %0A   
@@ -1109,325 +1109,8 @@
 ));%0A
-    //public static final Point2 PYRAMID_BACK_MIDDLE_AUTO = new Point2(PYRAMID_BACK_MIDDLE_NEW.getX() - (0.5 * PYRAMID_BACK_MIDDLE_GOAL_WIDTH),%0A    //        PYRAMID_BACK_MIDDLE_NEW.getY() - (0.87 * PYRAMID_BACK_MIDDLE_GOAL_HEIGHT));%0A    %0A    /*%0A     * TO RESET VT POINT: -0.5 goal width and -0.5 goal height%0A     */%0A
     
@@ -1203,69 +1203,54 @@
     
-super();%0A        fixedPoint = new Point2(xSetpoint, ySetpoint
+this(xSetpoint, ySetpoint, TargetType.ANY_GOAL
 );%0A 
@@ -1342,49 +1342,32 @@
 nt, 
-byte goalType) %7B%0A        super(goal
+TargetType target
 Type)
-;
+ %7B
 %0A   
@@ -1371,29 +1371,21 @@
         
-fixedPoint = 
+this(
 new Poin
@@ -1408,16 +1408,29 @@
 etpoint)
+, targetType)
 ;%0A    %7D%0A
@@ -1506,44 +1506,41 @@
     
-super();%0A        fixedPoint = point2
+this(point2, TargetType.ANY_GOAL)
 ;%0A  
@@ -1610,49 +1610,32 @@
 t2, 
-byte goalType) %7B%0A        super(goal
+TargetType target
 Type)
-;
+ %7B
 %0A   
@@ -1660,16 +1660,54 @@
 point2;%0A
+        this.targetType = targetType;%0A
     %7D%0A  
@@ -1739,39 +1739,24 @@
 etXSetpoint(
-double distance
 ) %7B%0A        
@@ -1830,23 +1830,8 @@
 int(
-double distance
 ) %7B%0A
@@ -1861,16 +1861,160 @@
 nt.getY(
+);%0A    %7D%0A    %0A    protected Target getTarget()%7B%0A        return getTargetCollection().getClosestTarget(getXSetpoint(), getYSetpoint(), targetType
 );%0A    %7D

Change FixedPointVisionTracking to use the new VisionTrackingCommand.