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;

/**
 * Aim the hood so that the camera aligns with the goal with fancy equations.
 */
public class AimShooter extends Command {

	NetworkTable table;
	double gravAcc = 32;
	double floorToTargetHeight = 8;
	double robotShooterToTargetHeight = 16;
	double wheelDiameter = 6;
	
    public AimShooter() {
    	requires(Robot.shooter);
    }

    protected void initialize() {
    	table = NetworkTable.getTable("Vision/report"); //TODO: Name this stuff.
    	setTimeout(10); // Should not take longer than 10 seconds.
    }

    protected void execute() {
    	//TODO: Horizontally align.
    	double distanceToTarget = table.getNumber("distance", 10);
    	
    	double angle = Math.atan(2*(floorToTargetHeight - (robotShooterToTargetHeight/12)/distanceToTarget) * 180/Math.PI);
    	double velocity = Math.sqrt((4*Math.pow(floorToTargetHeight-robotShooterToTargetHeight/12,2)+Math.pow(distanceToTarget, 2))*gravAcc/(2*(floorToTargetHeight-robotShooterToTargetHeight/12)));
    	double shaftRPM = velocity * 60/(Math.PI * wheelDiameter/12);
    	
    	Robot.shooter.setHoodAngle(angle);
    	Robot.shooter.setShootingWheelSpeed(shaftRPM);
    	
    	Robot.shooter.spinTurnTableAssisted();
    }

    protected boolean isFinished() {
    	return Robot.shooter.hoodPidController.onTarget()
    			&& Robot.shooter.hoodPidController.onTarget()
    			&& Robot.shooter.shootingWheelOnTarget()
    			|| isTimedOut();
    }

    protected void end() {
    }

    protected void interrupted() {
    }
}

@@ -885,18 +885,21 @@
 h.atan(2
-*(
+ * ( 
 floorToT
@@ -943,13 +943,17 @@
 ight
-/12)/
+ / 12) / 
 dist
@@ -971,17 +971,19 @@
 t) * 180
-/
+ / 
 Math.PI)
@@ -984,16 +984,22 @@
 th.PI);%0A
+    %09%0A
     %09dou
@@ -1027,11 +1027,14 @@
 qrt(
+ 
 (4
-*
+ * 
 Math
@@ -1049,33 +1049,35 @@
 orToTargetHeight
--
+ - 
 robotShooterToTa
@@ -1090,15 +1090,21 @@
 ight
-/12,2)+
+ / 12 , 2) + 
 Math
@@ -1132,21 +1132,29 @@
 , 2)
-)*
+ ) * 
 gravAcc
-/(2*
+ / ( 2 * 
 (flo
@@ -1169,17 +1169,19 @@
 etHeight
--
+ - 
 robotSho
@@ -1202,16 +1202,26 @@
 ight
-/12)
+ / 12 ) 
 ));%0A
+    %09%0A
     
@@ -1252,17 +1252,19 @@
 ity * 60
-/
+ / 
 (Math.PI
@@ -1279,17 +1279,19 @@
 Diameter
-/
+ / 
 12);%0A   

Fix spacing issues in AimShooter command.