/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.team4189.season2013.CB.commands;

import edu.wpi.first.wpilibj.command.PIDCommand;
import org.team4189.season2013.CB.subsystems.Shooter;
import org.team4189.season2013.CB.subsystems.VisionControl;
import org.team4189.season2013.CB.subsystems.VisionControl.TargetScores;
/**
 *
 * @author Developer
 */
public class AimControl extends PIDCommand {
    
    VisionControl visionControl;
    Shooter shooter;

    protected double returnPIDInput() {
        int goalWeWant = VisionControl.TargetScores.med;
        int goalWeUse = 1;
        TargetScores[] targets = visionControl.visionAnalyze();
        for(int i = 0; i < targets.length; i++){
            if(targets[i].goal == goalWeWant){
                goalWeUse=i; //select the value from the array that we want to use.
            }
        }
        
        
        
        return targets[goalWeUse].yValAim;
    }

    protected void usePIDOutput(double d) {
        shooter.adjustLauncher(d);
    }

    protected void initialize() {
        this.getPIDController().enable();
    }

    protected void execute() {
        //this.getPIDController();
    }

    protected boolean isFinished() {
        boolean isF = false;
        isF |= isTimedOut();
        isF |= this.getPIDController().onTarget();
        return isF;
    }

    protected  void end() {
        shooter.adjustLauncherStop();
        this.getPIDController().disable();
        this.getPIDController().free();
    }

    protected void interrupted() {
        shooter.adjustLauncherStop();
        this.getPIDController().disable();
    }

    public AimControl(){
        super(1,1,1,.2);
        this.setTimeout(30);
        this.setSetpoint(0);
        
    }
       
}
