package org.usfirst.frc.team619.logic.mapping;

import org.usfirst.frc.team619.hardware.Joystick;
import org.usfirst.frc.team619.hardware.TalonCan;
import org.usfirst.frc.team619.hardware.Talon;
import org.usfirst.frc.team619.logic.RobotThread;
import org.usfirst.frc.team619.logic.ThreadManager;
import org.usfirst.frc.team619.subsystems.DriverStation;
import org.usfirst.frc.team619.subsystems.drive.SwerveDriveBase;

/**
 * Map joystick values to motor values
 * @author CaRobotics
 */
public class SwerveDriveMappingThread extends RobotThread {
    protected SwerveDriveBase driveBase;
    protected TalonCan leftTalon;
    protected TalonCan rightTalon;
    protected TalonCan leftTalon2, rightTalon2;
    protected Talon leftTalonTurn;
    protected Talon rightTalonTurn;
    protected Talon leftTalonTurn2, rightTalonTurn2;
    protected DriverStation driverStation;
    private boolean firstError = true;
    private final static boolean DEBUG = true;
    private long lastTime = 0;

    public SwerveDriveMappingThread(SwerveDriveBase driveBase,
            DriverStation driverStation, int period, ThreadManager threadManager) {
        super(period, threadManager);
        this.driveBase = driveBase;
        this.driverStation = driverStation;
    }

    protected void cycle() {
        double scalePercent = driverStation.getLeftJoystick().getAxis(Joystick.Axis.AXIS_Z);
        
        if(scalePercent < 0.2){
            scalePercent = 0.2;
        }
        
        double xAxis = driverStation.getLeftJoystick().getAxis(Joystick.Axis.AXIS_X);
        double yAxis = driverStation.getLeftJoystick().getAxis(Joystick.Axis.AXIS_Y);
        //when turning Saitek Aviator Joystick that is the throttle axis as determined using the Joystick Explorer UI developed by FIRST
        //double xRightAxis = driverStation.getRightJoystick().getAxis(Joystick.Axis.AXIS_X);
        
        //gets percentages (numbers from -1 to 1) from the joystick's axes used for driving
        double drivePercent = yAxis * scalePercent;
        //double spinPercent = xRightAxis * scalePercent;
        double turnPercent = xAxis * scalePercent;
        
        
        //
        //TODO: figure out logic regarding each wheel when rotating
        //double leftPercent = spinPercent;
        //double rightPercent = spinPercent;
        //double leftPercent2 = spinPercent;
        //double rightPercent2 = spinPercent;
        
        try {
            if(xAxis > -0.2 && xAxis < 0.2 && yAxis > -0.2 && yAxis < 0.2){
                driveBase.getLeftTalon().set(0);
                driveBase.getRightTalon().set(0);
                driveBase.getLeftTalon2().set(0);
                driveBase.getRightTalon2().set(0);
                driveBase.getLeftTurn().set(0);
                driveBase.getRightTurn().set(0);
                driveBase.getLeftTurn2().set(0);
                driveBase.getRightTurn2().set(0);
            }else{
                driveBase.getLeftTalon().set(drivePercent);
                driveBase.getRightTalon().set(-drivePercent);
                driveBase.getLeftTalon2().set(drivePercent);
                driveBase.getRightTalon2().set(-drivePercent);
                driveBase.getLeftTurn().set(turnPercent);
                driveBase.getRightTurn().set(-turnPercent);
                driveBase.getLeftTurn2().set(turnPercent);
                driveBase.getRightTurn2().set(-turnPercent);
            }
        
            //Orientation
            //0-----1 ^
            //|     | |
            //|     |
            //2-----3
            
            if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON5)){
                driveBase.getLeftTurn().set(turnPercent * 0.25);
            }
            
            if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON6)){
                driveBase.getRightTurn().set(turnPercent * 0.25);
            }
            
            if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON7)){
                driveBase.getLeftTurn2().set(turnPercent * 0.25);
            }
            
            if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON8)){
                driveBase.getRightTurn2().set(turnPercent * 0.25);
            }
            
        } catch (Exception e) {
            if (firstError || DEBUG) {
                e.printStackTrace();
            }
        }
        
        if(DEBUG){
        //Write debug code    
        }
            
    }

}

@@ -411,16 +411,81 @@
 iveBase;
+%0A//import org.usfirst.frc.team619.subsystems.drive.TurndriveBase;
 %0A%0A/**%0A *
@@ -607,16 +607,18 @@
 hread %7B%0A
+//
     prot
@@ -623,16 +623,52 @@
 otected 
+TurndriveBase driveBase;%0A%09protected 
 SwerveDr
@@ -697,35 +697,32 @@
  protected Talon
-Can
  leftTalon;%0A    
@@ -736,19 +736,16 @@
 ed Talon
-Can
  rightTa
@@ -768,19 +768,16 @@
 ed Talon
-Can
  leftTal
@@ -805,32 +805,35 @@
  protected Talon
+Can
  leftTalonTurn;%0A
@@ -843,32 +843,35 @@
  protected Talon
+Can
  rightTalonTurn;
@@ -882,32 +882,35 @@
  protected Talon
+Can
  leftTalonTurn2,
@@ -1060,39 +1060,71 @@
 ue;%0A
+%0A//
     p
-rivate long lastTime = 0;%0A
+ublic SwerveDriveMappingThread(TurndriveBase driveBase,
 %0A   
@@ -1305,16 +1305,18 @@
 nager);%0A
+//
         
@@ -1784,145 +1784,8 @@
 Y);%0A
-        //when turning Saitek Aviator Joystick that is the throttle axis as determined using the Joystick Explorer UI developed by FIRST%0A
     
@@ -1895,91 +1895,8 @@
     
-//gets percentages (numbers from -1 to 1) from the joystick's axes used for driving
 %0A   
@@ -2066,284 +2066,8 @@
    %0A
-        %0A        //%0A        //TODO: figure out logic regarding each wheel when rotating%0A        //double leftPercent = spinPercent;%0A        //double rightPercent = spinPercent;%0A        //double leftPercent2 = spinPercent;%0A        //double rightPercent2 = spinPercent;%0A        
 %0A   
@@ -2099,25 +2099,25 @@
 (xAxis %3E -0.
-2
+3
  && xAxis %3C 
@@ -2118,17 +2118,17 @@
 xis %3C 0.
-2
+3
  && yAxi
@@ -2134,17 +2134,17 @@
 is %3E -0.
-2
+3
  && yAxi
@@ -2145,25 +2145,25 @@
 & yAxis %3C 0.
-2
+3
 )%7B%0A         
@@ -2169,1756 +2169,69 @@
     
-  
-  driveBase.getLeftTalon().set(0);%0A                driveBase.getRightTalon().set(0);%0A                driveBase.getLeftTalon2().set(0);%0A                driveBase.getRightTalon2().set(0);%0A                driveBase.getLeftTurn().set(0);%0A                driveBase.getRightTurn().set(0);%0A                driveBase.getLeftTurn2().set(0);%0A                driveBase.getRightTurn2().set(0);%0A            %7Delse%7B%0A                driveBase.getLeftTalon().set(drivePercent);%0A                driveBase.getRightTalon().set(-drivePercent);%0A                driveBase.getLeftTalon2().set(drivePercent);%0A                driveBase.getRightTalon2().set(-drivePercent);%0A                driveBase.getLeftTurn().set(turnPercent);%0A                driveBase.getRightTurn().set(-turnPercent);%0A                driveBase.getLeftTurn2().set(turnPercent);%0A                driveBase.getRightTurn2().set(-turnPercent);%0A            %7D%0A        %0A            //Orientation%0A            //0-----1 %5E%0A            //%7C     %7C %7C%0A            //%7C     %7C%0A            //2-----3%0A            %0A            if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON5))%7B%0A                driveBase.getLeftTurn().set(turnPercent * 0.25);%0A            %7D%0A            %0A            if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON6))%7B%0A                driveBase.getRightTurn().set(turnPercent * 0.25);%0A            %7D%0A            %0A            if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON7))%7B%0A                driveBase.getLeftTurn2().set(turnPercent * 0.25);%0A            %7D%0A            %0A            if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON8))%7B%0A                driveBase.getRightTurn2().set(turnPercent * 0.25);%0A            %7D%0A            
+%09%0A                %7Delse%7B%0A                %09%0A                %7D%0A
 %0A   

Update SwerveDriveMappingThread.java