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

import org.usfirst.frc.team619.hardware.Joystick;
import org.usfirst.frc.team619.hardware.CANTalon;
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;

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

    public SwerveDriveMappingThread(DriverStation driverStation, int period, ThreadManager threadManager) {
        super(period, threadManager);
        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.3 && xAxis < 0.3 && yAxis > -0.3 && yAxis < 0.3){
                leftTalonTurn.set(0);
                rightTalonTurn.set(0);
                leftTalonTurn2.set(0);
                rightTalonTurn2.set(0);
            }else if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON5)){
            	leftTalonTurn.set(turnPercent);
            }else if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON6)){
            	rightTalonTurn.set(turnPercent);
            }else if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON7)){
            	leftTalonTurn2.set(turnPercent);
            }else if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON8)){
            	rightTalonTurn2.set(turnPercent);          	
            }else{
/*                turnBase.getLeftTalon().set(drivePercent * 0.25);
                turnBase.getRightTalon().set(-drivePercent * 0.25);
                turnBase.getLeftTalon2().set(drivePercent * 0.25);
                turnBase.getRightTalon2().set(-drivePercent * 0.25);
*/                leftTalonTurn.set(turnPercent);
                rightTalonTurn.set(-turnPercent);
                leftTalonTurn2.set(turnPercent);
                rightTalonTurn2.set(-turnPercent);
                }
        
            //Orientation
            //0-----1 ^
            //|     | |
            //|     |
            //2-----3
            
        } catch (Exception e) {
            if (firstError || DEBUG) {
                e.printStackTrace();
            }
        }
        
        if(DEBUG){
        //Write debug code    
        }
            
    }

}

@@ -495,233 +495,33 @@
 ted 
-Talon leftTalon;%0A    protected Talon rightTalon;%0A    protected Talon leftTalon2, rightTalon2;%0A    protected CANTalon leftTalonTurn;%0A    protected CANTalon rightTalonTurn;%0A    protected CANTalon leftTalonTurn2, rightTalonTurn2
+SwerveDriveBase driveBase
 ;%0A  
@@ -565,47 +565,8 @@
 on;%0A
-    private boolean firstError = true;%0A
     
@@ -755,16 +755,52 @@
 nager);%0A
+        this.driveBase = driveBase;%0A
         
@@ -1240,147 +1240,8 @@
     
-//when turning Saitek Aviator Joystick that is the throttle axis as determined using the Joystick Explorer UI developed by FIRST%0A        //
 doub
@@ -1436,28 +1436,18 @@
  double 
-drivePercent
+LY
  = yAxis
@@ -1475,37 +1475,21 @@
     
-//
 double 
-spinPercent = xRight
+LX = x
 Axis
@@ -1524,709 +1524,38 @@
 ble 
-turnPercent = xAxis * scalePercent;%0A        %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        try %7B%0A            if(xAxis %3E -0.3 && xAxis %3C 0.3 && yAxis %3E -0.3 && yAxis %3C 0.3)%7B%0A                leftTalonTurn.set(0);%0A                rightTalonTurn.set(0);%0A                leftTalonTurn2.set(0);%0A                rightTalonTurn2.set(0);%0A            %7Delse if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON5))%7B%0A            %09leftTalonTurn.set(turn
+RX = xRightAxis * scale
 Percent
-)
 ;%0A  
@@ -1564,1200 +1564,53 @@
     
-    %7Delse if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON6))%7B%0A            %09rightTalonTurn.set(turnPercent);%0A            %7Delse if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON7))%7B%0A            %09leftTalonTurn2.set(turnPercent);%0A            %7Delse if(driverStation.getLeftJoystick().getButton(Joystick.Button.BUTTON8))%7B%0A            %09rightTalonTurn2.set(turnPercent);          %09%0A            %7Delse%7B%0A/*                turnBase.getLeftTalon().set(drivePercent * 0.25);%0A                turnBase.getRightTalon().set(-drivePercent * 0.25);%0A                turnBase.getLeftTalon2().set(drivePercent * 0.25);%0A                turnBase.getRightTalon2().set(-drivePercent * 0.25);%0A*/                leftTalonTurn.set(turnPercent);%0A                rightTalonTurn.set(-turnPercent);%0A                leftTalonTurn2.set(turnPercent);%0A                rightTalonTurn2.set(-turnPercent);%0A                %7D%0A        %0A            //Orientation%0A            //0-----1 %5E%0A            //%7C     %7C %7C%0A            //%7C     %7C%0A            //2-----3%0A            %0A        %7D catch (Exception e) %7B%0A            if (firstError %7C%7C DEBUG) %7B%0A                e.printStackTrace();%0A            %7D%0A        %7D
+%0A        driveBase.move(LY, LX, RX);%0A        
 %0A   

Update SwerveDriveMappingThread.java