/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

package org.usfirst.frc330.Beachbot2014Java.commands;

import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc330.Beachbot2014Java.Robot;

/**
 *
 * @author Joe-XPS13-W7
 */
public abstract class MoveArmCommand extends Command {
    double setpoint; 
    boolean started = false;
    double outputRange = 0;

    public MoveArmCommand(double position) {
        requires(Robot.arm);
        requires(Robot.wings);
        setpoint = position;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        if (!Robot.arm.areWingsSafeToClose(setpoint))
            Robot.wings.setWingsOpen();
        started = false;

    }

    // Called repeatedly when this Command is scheduled to run
    final protected void execute() {
        if ((Robot.wings.areWingsOpen() || Robot.arm.areWingsSafeToClose(setpoint)) && !started) {
                Robot.arm.setArmSetPoint(setpoint);
                Robot.arm.enable();
                started = true;
                outputRange = 0.10;
                Robot.arm.setPIDOutputRange(outputRange);
//                System.out.println("outputRange: " + outputRange);                
        } else if (started) {
            //TODO make outputRange step a Preference
            outputRange = outputRange + 0.10;
            if (outputRange > 0.8)
                outputRange = 0.8;
            Robot.arm.setPIDOutputRange(outputRange);
//            System.out.println("outputRange: " + outputRange);
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    final protected boolean isFinished() {
        return Robot.arm.onTarget() && started;
    }

    // Called once after isFinished returns true
    final protected void end() {
        Robot.arm.setPIDOutputRangeDefault();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    final protected void interrupted() {
        end();
    }
    
}

@@ -504,141 +504,822 @@
  0;%0A
-%0A    public MoveArmCommand(double position) %7B%0A        requires(Robot.arm);%0A        requires(Robot.wings);%0A        setpoint = position
+    double startPosition = 0;%0A    double accelDistance = 0;%0A    double decelDistance = 0;%0A    double maxSpeed = 0;%0A    double minSpeed = 0;%0A%0A    public MoveArmCommand(double position) %7B%0A        this(position, 1.0, 0.1, 0.1, 0.1);%0A    %7D%0A    %0A//maxSpeed       _______%0A//              /       %5C%0A//             /         %5C%0A//minSpeed    %7C           %5C%0A//           accel      decel%0A//         Distance    Distance%0A    %0A    public MoveArmCommand(double position, double maxSpeed, double minSpeed, double accelDistance, double decelDistance) %7B%0A        requires(Robot.arm);%0A        requires(Robot.wings);%0A        setpoint = position;%0A        this.maxSpeed = maxSpeed;%0A        this.minSpeed = minSpeed;%0A        this.maxSpeed = maxSpeed;%0A        this.accelDistance = accelDistance;%0A        this.decelDistance = decelDistance
 ;%0A  
@@ -1535,16 +1535,205 @@
  false;%0A
+        startPosition = Robot.arm.getArmPosition();%0A        if (setpoint - startPosition %3C accelDistance+decelDistance)%0A            decelDistance = setpoint - accelDistance - startPosition;
 %0A    %7D%0A%0A
@@ -1828,24 +1828,102 @@
 execute() %7B%0A
+        double armPosition = Robot.arm.getArmPosition();%0A        double x, y;%0A
         if (
@@ -2091,17 +2091,37 @@
 arm.
-enable(
+setPIDOutputRange(minSpeed
 );
+ 
 %0A   
@@ -2137,120 +2137,100 @@
     
-started = true;%0A                outputRange = 0.10;%0A                Robot.arm.setPIDOutputRange(outputRange);%0A//
+outputRange = minSpeed;%0A                Robot.arm.enable();%0A                started = true;%0A
     
@@ -2354,50 +2354,287 @@
     
-//TODO make outputRange step a Preference%0A
+if (Robot.arm.getArmPosition() %3E setpoint) %7B%0A                outputRange = minSpeed;%0A            %7D else if (Robot.arm.getArmPosition() %3C= startPosition + accelDistance) %7B%0A                x = (armPosition - startPosition)/accelDistance;%0A                y = maxSpeed - minSpeed;%0A  
     
@@ -2633,32 +2633,34 @@
 ;%0A              
+  
 outputRange = ou
@@ -2661,26 +2661,161 @@
 e = 
-outputRange + 0.10
+y*x+minSpeed;%0A            %7D else if (Robot.arm.getArmPosition() %3E= setpoint - decelDistance) %7B%0A                x = (setpoint - armPosition)/decelDistance
 ;%0A  
@@ -2824,20 +2824,20 @@
         
-if (
+    
 outputRa
@@ -2844,49 +2844,96 @@
 nge 
-%3E 0.8)%0A                outputRange = 0.8;
+= x*maxSpeed;%0A            %7D else %7B%0A               outputRange = maxSpeed;%0A            %7D%0A
 %0A   
@@ -2983,18 +2983,16 @@
 Range);%0A
-//
         

add trapezoidal motion profile