
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.InternalButton;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.templates.commands.highlevel.*;
import edu.wpi.first.wpilibj.templates.commands.lowlevel.*;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
        
    private Joystick stick;
    private JoystickButton trigger;
    private JoystickButton bttn11; 
    private JoystickButton bttn12;
    private JoystickButton bttn2;
    private JoystickButton bttn3;
    private JoystickButton bttn4;
    private JoystickButton bttn5;
    private JoystickButton bttn6;
  
    
    private Joystick stick2;
    private JoystickButton S2trigger;
    private JoystickButton S2bttn8;
    private JoystickButton S2bttn10;
    private JoystickButton S2bttn11;
    private JoystickButton S2bttn12;
    //stick 2, button 7 is taken in the H_BallCheck command
    private JoystickButton S2bttn6;
    private JoystickButton S2bttn4;
    private JoystickButton S2bttn2;
    private JoystickButton S2bttn3;
    private JoystickButton S2bttn5;
    
    
    
    InternalButton button1 = new InternalButton();
    InternalButton button2 = new InternalButton();
    InternalButton button3 = new InternalButton();
    InternalButton button4 = new InternalButton();
    InternalButton button5 = new InternalButton();
   
    
    public OI(){
        
        stick = new Joystick(RobotMap.RIGHT_JOYSTICK_PORT);
        stick2 = new Joystick(RobotMap.LEFT_JOYSTICK_PORT);

        //button1.whenPressed(new L_Chassis_ShiftGears());
        
        S2trigger = new JoystickButton(stick2, Joystick.ButtonType.kTop.value);
        S2bttn8 = new JoystickButton(stick2, 8);
        S2bttn10 = new JoystickButton(stick2, 10);
        S2bttn11 = new JoystickButton(stick2, 11);
        S2bttn12 = new JoystickButton(stick2, 12);
        S2bttn6 = new JoystickButton(stick2, 6);
        S2bttn4 = new JoystickButton(stick2, 4);
        S2bttn2 = new JoystickButton(stick2, 2);
        S2bttn3 = new JoystickButton(stick2, 3);
        S2bttn5 = new JoystickButton(stick2, 5);
                
        trigger = new JoystickButton(stick, Joystick.ButtonType.kTop.value);
        bttn11 = new JoystickButton(stick, 11);
        bttn12 = new JoystickButton(stick, 12);
        bttn2 = new JoystickButton(stick, 2);
        bttn3 = new JoystickButton(stick, 3);
        bttn4 = new JoystickButton(stick, 4);
        bttn5 = new JoystickButton(stick, 5);
        bttn6 = new JoystickButton(stick, 6);


        //S2trigger.whenPressed(new H_elevatorShooterFeed());
        //trigger on stick 2 is used in H_BallHandle to shoot
        //S2bttn8.whenPressed(new H_ShooterPreset(RobotMap.FENDER_MOTOR_SPEED, RobotMap.FENDER_MOTOR_ANGLE, RobotMap.FENDER_MOTOR_RATIO, 1));
        //S2bttn10.whenPressed(new H_ShooterPreset(RobotMap.KEY_MOTOR_SPEED, RobotMap.KEY_MOTOR_ANGLE, RobotMap.KEY_MOTOR_RATIO, 1));
        //S2bttn12.whenPressed(new H_ShooterPreset(RobotMap.LANE_MOTOR_SPEED, RobotMap.LANE_MOTOR_ANGLE, RobotMap.LANE_MOTOR_RATIO, 1));
        //stick 2, button 7 is taken in the H_BallCheck command (sets numBalls to 0)
        S2bttn11.whileHeld(new H_ShooterPreset(RobotMap.FENDER_MOTOR_SPEED, RobotMap.FENDER_MOTOR_ANGLE, RobotMap.FENDER_MOTOR_RATIO, 1));
        if(stick2.getRawButton(11) && RobotMap.S2bttn11Pressed == false)
            RobotMap.S2bttn11Pressed = true;
        else if (!stick2.getRawButton(11) && RobotMap.S2bttn11Pressed == true)
            RobotMap.S2bttn11Pressed = false;
        S2bttn6.whenPressed(new L_Intake_In()); 
        S2bttn3.whenPressed(new L_Elevator_Stop()); 
        S2bttn4.whenPressed(new L_Intake_Off()); 
        S2bttn5.whenPressed(new L_Elevator_Up());
        S2bttn8.whenPressed(new H_BallHandle());
        S2bttn2.whileHeld(new H_ShooterPreset(RobotMap.KEY_MOTOR_SPEED, RobotMap.KEY_MOTOR_ANGLE, RobotMap.KEY_MOTOR_RATIO, 1));
        if(stick2.getRawButton(2) && RobotMap.S2bttn2Pressed == false)
            RobotMap.S2bttn2Pressed = true;
        else if (!stick2.getRawButton(2) && RobotMap.S2bttn2Pressed == true)
            RobotMap.S2bttn2Pressed = false;
        
        trigger.whenPressed(new H_BridgeArm_ChangePosition(3)); //toggles the arm
        bttn11.whenPressed(new L_Shooter_PIDSet(false));
        bttn12.whenPressed(new L_Shooter_PIDSet(true));
        bttn2.whenPressed(new L_Chassis_ToggleFront());      
        //bttn4.whenPressed(new L_Intake_In());
        //bttn6.whenPressed(new L_Intake_Off());  
        //bttn5.whenPressed(new L_Elevator_Stop());
        bttn3.whenPressed(new L_Chassis_ShiftGears());//whenPressed(new L_Elevator_Up());
       
    }
    
    public Joystick getJoystick(){
        return stick;
    }
    
    public Joystick getJoystick2() {
        return stick2;
    }
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
    // Joystick stick = new Joystick(port);
    // Button button = new JoystickButton(stick, buttonNumber);
    
    // Another type of button you can create is a DigitalIOButton, which is
    // a button or switch hooked up to the cypress module. These are useful if
    // you want to build a customized operator interface.
    // Button button = new DigitalIOButton(1);
    
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenPressed(new ExampleCommand());
    
    // Run the command while the button is being held down and interrupt it once
    // the button is released.
    // button.whileHeld(new ExampleCommand());
    
    // Start the command when the button is released  and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenReleased(new ExampleCommand());
}

/*********** Tee hee hee......
 *******************/