package com.edinarobotics.zed.commands;

import com.edinarobotics.zed.Components;
import com.edinarobotics.zed.subsystems.DrivetrainRotation;
import com.edinarobotics.zed.vision.PIDTargetX;
import com.edinarobotics.zed.vision.PIDTargetY;
import com.edinarobotics.zed.vision.TargetCollection;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.networktables.NetworkTable;

public class VisionTrackingCommand extends Command {
    private NetworkTable visionTable = NetworkTable.getTable("vision");
    private TargetCollection targetCollection;
    public PIDController pidControllerX;
    public PIDController pidControllerY;
    public PIDTargetX pidTargetX;
    public PIDTargetY pidTargetY;
    public DrivetrainRotation drivetrainRotation;
    
    private final double X_P = 1;
    private final double X_I = 0;
    private final double X_D = 0;
    private final double Y_P = 1;
    private final double Y_I = 1;
    private final double Y_D = 1;
    
    public VisionTrackingCommand() {
        drivetrainRotation = Components.getInstance().drivetrainRotation;
        pidTargetX = new PIDTargetX();
        pidTargetY = new PIDTargetY();
        pidControllerX = new PIDController(X_P, X_I, X_D, pidTargetX, drivetrainRotation);
        pidControllerY = new PIDController(Y_P, Y_I, Y_D, pidTargetY, drivetrainRotation);
    }
    
    protected void initialize() {
        visionTable.putBoolean("vton", true);
        pidControllerX.enable();
        pidControllerY.enable();
    }

    protected void execute() {
        targetCollection = new TargetCollection(visionTable.getString("vtdata"));
        pidControllerX.setSetpoint(0);
        pidControllerY.setSetpoint(0);
    }

    protected boolean isFinished() {
        return false;
    }

    protected void end() {
        visionTable.putBoolean("vton", false);
        pidControllerX.disable();
        pidControllerY.disable();
    }

    protected void interrupted() {
        end();
    }
    
}

@@ -34,16 +34,115 @@
 mands;%0A%0A
+import com.edinarobotics.utils.pid.PIDConfig;%0Aimport com.edinarobotics.utils.pid.PIDTuningManager;%0A
 import c
@@ -174,16 +174,16 @@
 onents;%0A
-
 import c
@@ -900,24 +900,92 @@
 inRotation;%0A
+    private PIDConfig xPIDConfig;%0A    private PIDConfig yPIDConfig;%0A
     %0A    pri
@@ -1553,24 +1553,196 @@
 nRotation);%0A
+        xPIDConfig = PIDTuningManager.getInstance().getPIDConfig(%22Vision Horizontal%22);%0A        yPIDConfig = PIDTuningManager.getInstance().getPIDConfig(%22Vision Vertical%22);%0A
     %7D%0A    %0A 
@@ -2041,24 +2041,24 @@
 etpoint(0);%0A
-
         pidC
@@ -2080,24 +2080,468 @@
 etpoint(0);%0A
+        //PID tuning code%0A        pidControllerX.setPID(xPIDConfig.getP(X_P), xPIDConfig.getI(X_I), xPIDConfig.getD(X_D));%0A        xPIDConfig.setSetpoint(pidControllerX.getSetpoint());%0A        xPIDConfig.setValue(pidTargetX.pidGet());%0A        pidControllerY.setPID(yPIDConfig.getP(Y_P), yPIDConfig.getI(Y_I), yPIDConfig.getD(Y_D));%0A        yPIDConfig.setSetpoint(pidControllerY.getSetpoint());%0A        yPIDConfig.setValue(pidTargetY.pidGet());%0A
     %7D%0A%0A    p

Add PID tuning code to VisionTrackingCommand.