/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.usfirst.frc330.Beachbot2013Java.commands;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.command.AutoSpreadsheetCommand;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc330.Beachbot2013Java.Robot;
/*
 * $Log$
 */
/**
 * Drive the robot to a specified waypoint (relative to where the robot started) 
 * using the encoders and the gyro to keep straight and ramp up to full speed. Finish when
 * one of the encoders is within the specified {@link #setParam3(double) tolerance}. 
 * The {@link #setParam2(double) Y} location is positive to the front of the robot. 
 * The {@link #setParam1(double) X} location is positive to the right of the robot.
 * The robot's current angle should be close to the angle to drive to the waypoint
 * (ie this command is not appropriate for both turning and driving).
 * If the robot needs to turn prior to driving to the waypoint, use {@link TurnWaypoint} 
 * first.
 * <p>
 * For example, to drive 10 feet forward from where the robot started, set Y to 
 * 120 (inches) and set X to 0.
 * A reasonable tolerance is 3 inches for normal movements. This will stop the robot
 * when it is between 117 - 123 inches. If a smaller
 * tolerance is used, the robot may not ever reach the tolerance, and the 
 * {@link #setTimeout(double) timeout} may be exceeded. This will slow down the
 * execution of future commands.
 * 
 * @see DriveEncoderGyroRamp
 * @see TurnGyroWaypoint
 * 
 * @author Joe
 */
public class DriveWaypoint extends DriveEncoderGyroRampRel {
    double x,y;
    public DriveWaypoint(double x, double y, double tolerance, double timeout, boolean stopAtEnd) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        super();
        this.x = x;
        this.y = y;
        calcXY(x, y);
        this.setParam3(tolerance);
        this.setStopAtEnd(stopAtEnd);
        this.setTimeout(timeout);
    }

    protected void initialize() {
        calcXY(x,y);
        super.initialize();
    }
    
    
    
     /**
     * The first parameter in the AutoSpreadsheet, X
     * @param X The X component of the waypoint
     */
    public void setParam1(double x) {
        this.x = x;
    }
    /**
     * The second parameter in the AutoSpreadsheet, Y
     * @param Y The Y component of the waypoint
     */
    public void setParam2(double y) {
        this.y = y;
    }

    private void calcXY(double x, double y) {
        double curX, curY, deltaX, deltaY, calcAngle, calcDistance;
        
        curX = Robot.chassis.getX();
        curY = Robot.chassis.getY();
        
        deltaX = x - curX;
        deltaY = y - curY;
        
        calcDistance = Math.sqrt(deltaX*deltaX+deltaY*deltaY);
        calcAngle = Math.toDegrees(MathUtils.atan2(deltaX, deltaY));
        
        if (Math.abs(Robot.chassis.getAngle()-calcAngle)<180)
        {
            calcAngle=calcAngle;
        }
        else if (Robot.chassis.getAngle() > calcAngle)
        {
            calcAngle += 360;
        }
        else
        {
            calcAngle -= 360;
        }
        System.out.println("distance: " + calcDistance);
        System.out.println("angle: " + calcAngle);
        
        super.setParam1(calcDistance);
        super.setParam2(calcAngle);
    }
    
    public Command copy() {
        return new DriveWaypoint(0,0,0,0,false);
    }

}

@@ -353,17 +353,106 @@
 %0A * $Log
-$
+: DriveWaypoint.java,v $%0A * Revision 1.3  2013-02-17 02:53:43  jross%0A * update javadocs%0A *
 %0A */%0A/**
@@ -2019,28 +2019,29 @@
 y);%0A        
-this
+super
 .setParam3(t
@@ -2037,17 +2037,17 @@
 setParam
-3
+2
 (toleran
@@ -2593,24 +2593,599 @@
 y = y;%0A    %7D
+%0A    %0A     /**%0A     * The third parameter in the AutoSpreadsheet, tolerance. %0A     * The tolerance in inches for how close to be to the distance before stopping.%0A     * 3 inches is a reasonable tolerance for normal movements. If a smaller%0A     * tolerance is used, the robot may not ever reach the tolerance, and the %0A     * %7B@link #setTimeout(double) timeout%7D may be reached.%0A     * @param tolerance in inches%0A     * @see edu.wpi.first.wpilibj.PIDController#setAbsoluteTolerance%0A     */%0A    public void setParam3(double tolerance) %7B%0A        super.setParam2(tolerance);%0A    %7D
 %0A%0A    privat
@@ -4045,17 +4045,17 @@
 setParam
-2
+3
 (calcAng

fix tolerance and passing angle parameters