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

import com.sun.squawk.util.MathUtils;

/**
 *
 * @author FIRST
 */
public class SwerveVector {
    private SwerveModule swerve;
    private double Px, Py;
    private double hyp, xOffset = 0.0, yOffset = -25;
    private double desiredPower = 0;
    private double handsOffAngle = 0;
    
    public SwerveVector(SwerveModule _swerve, double xPosition, double yPosition, double _handsOffAngle) {
        swerve = _swerve;
        Px = xPosition;
        Py = yPosition;
        hyp = Math.sqrt((Px+xOffset)*(Px+xOffset) + (Py+yOffset)*(Py+yOffset));
        handsOffAngle = _handsOffAngle;
    }
    
    double Rx(double rotation) {
        return (rotation * (Py + yOffset) / hyp);
    }
    double Ry(double rotation) {
        return (rotation * -(Px + xOffset) / hyp);
    }
    public void setXOffset(double newX) {
        xOffset = newX;
        hyp = Math.sqrt((Px+xOffset)*(Px+xOffset) + (Py+yOffset)*(Py+yOffset));
    }
    public void setYOffset(double newY) {
        yOffset = newY;
        hyp = Math.sqrt((Px+xOffset)*(Px+xOffset) + (Py+yOffset)*(Py+yOffset));
    }
    public double drive(double x, double y, double r) {
        double vecX, vecY;
        double cappedTurn = r * 0.7;
        vecX = x + Rx(cappedTurn);
        vecY = y + Ry(cappedTurn);
        // Convert vecX and vecY to polar coords
        double angle, magnitude;
        angle = MathUtils.atan2(-vecX, vecY);  // we want 0 degrees to be in the Positive Y direction, which is towards the front of the robot
        swerve.setTwist(angle);
        desiredPower = Math.sqrt(vecX*vecX+vecY*vecY);
        return desiredPower;
    }
    public void SetMaxPower( double maxPower )
    {
        swerve.setSpeed( desiredPower / maxPower );
    }
    
    public void HandsOff()
    {
        swerve.setTwist( handsOffAngle );
        swerve.setSpeed(0.0);
    }
}

@@ -325,21 +325,20 @@
 e hyp, x
-Offse
+Pivo
 t = 0.0,
@@ -343,20 +343,17 @@
 0, y
-Offse
+Pivo
 t = 
--25
+0
 ;%0D%0A 
@@ -644,37 +644,36 @@
 h.sqrt((Px+x
-Offse
+Pivo
 t)*(Px+x
 Offset) + (P
@@ -652,37 +652,36 @@
 Px+xPivot)*(Px+x
-Offse
+Pivo
 t) + (Py+yOffset
@@ -670,37 +670,36 @@
 vot) + (Py+y
-Offse
+Pivo
 t)*(Py+y
 Offset));%0D%0A 
@@ -678,37 +678,36 @@
 Py+yPivot)*(Py+y
-Offse
+Pivo
 t));%0D%0A        ha
@@ -818,21 +818,20 @@
  (Py + y
-Offse
+Pivo
 t) / hyp
@@ -910,21 +910,20 @@
 -(Px + x
-Offse
+Pivo
 t) / hyp
@@ -985,21 +985,20 @@
        x
-Offse
+Pivo
 t = newX
@@ -1025,37 +1025,36 @@
 h.sqrt((Px+x
-Offse
+Pivo
 t)*(Px+x
 Offset) + (P
@@ -1033,37 +1033,36 @@
 Px+xPivot)*(Px+x
-Offse
+Pivo
 t) + (Py+yOffset
@@ -1051,37 +1051,36 @@
 vot) + (Py+y
-Offse
+Pivo
 t)*(Py+y
 Offset));%0D%0A 
@@ -1059,37 +1059,36 @@
 Py+yPivot)*(Py+y
-Offse
+Pivo
 t));%0D%0A    %7D%0D%0A   
@@ -1136,21 +1136,20 @@
        y
-Offse
+Pivo
 t = newY
@@ -1184,21 +1184,20 @@
 Px+x
-Offse
+Pivo
 t)*(Px+x
 Offs
@@ -1192,21 +1192,20 @@
 t)*(Px+x
-Offse
+Pivo
 t) + (Py
@@ -1210,21 +1210,20 @@
 Py+y
-Offse
+Pivo
 t)*(Py+y
 Offs
@@ -1218,21 +1218,20 @@
 t)*(Py+y
-Offse
+Pivo
 t));%0D%0A  

Set Pivot back to 0,0 center of robot.