package org.robockets.stronghold.robot.highgoalshooter;

import org.robockets.stronghold.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * Align the robot horizontally with the target.
 */
public class HorizontalAlign extends Command {

	NetworkTable table;
	boolean continuous;
	
	/**
	 * * @param continuos		If it should stop when on target.
	 */
    public HorizontalAlign(boolean continuous) {
    	requires(Robot.shooter);
    	this.continuous = continuous;
    }

    protected void initialize() {
    	table = NetworkTable.getTable("vision");
    }

    protected void execute() {
    	double pixelError = table.getNumber("horiz_offset", 0);
    	SmartDashboard.putNumber("factorz", SmartDashboard.getNumber("factorz", 0.02));
		double factor = SmartDashboard.getNumber("factorz", 0.02); // Or something.
    	
		SmartDashboard.putNumber("Setpoint delta", factor * pixelError);
		if (table.getBoolean("heartbeat", false)) {
			Robot.shooter.setTurnTableAngle(Robot.shooter.turnTableSource.pidGet() + (factor * pixelError));
			table.putBoolean("heartbeat", false);
		}
    }

    protected boolean isFinished() {
    	if (continuous) return Robot.shooter.turnTablePidController.onTarget();
    	return false;
    }

    protected void end() {
    }

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

@@ -1027,23 +1027,22 @@
 able.get
-Boolean
+Number
 (%22heartb
@@ -1047,22 +1047,23 @@
 tbeat%22, 
-false)
+0) == 1
 ) %7B%0A%09%09%09R
@@ -1174,15 +1174,14 @@
 .put
-Boolean
+Number
 (%22he
@@ -1190,21 +1190,17 @@
 tbeat%22, 
-false
+1
 );%0A%09%09%7D%0A 

Make heartbeat work with numbers.