package com.edinarobotics.zed;

import com.edinarobotics.utils.gamepad.Gamepad;
import com.edinarobotics.zed.commands.*;
import com.edinarobotics.zed.subsystems.Auger;
import com.edinarobotics.zed.subsystems.Collector;
import com.edinarobotics.zed.subsystems.Conveyor;
import com.edinarobotics.zed.subsystems.Lifter;
import com.edinarobotics.zed.subsystems.Shooter;

/**
 * Controls handles creating the {@link Gamepad} objects
 * used to control the robot as well as binding the proper Commands
 * to button actions.
 */
public class Controls {
    private static Controls instance;
    private static final double ONE_JOYSTICK_MAGNITUDE = 1;
    
    public final Gamepad gamepad1;
    public final Gamepad gamepad2;
    
    private Controls(){
        gamepad1 = new Gamepad(1);
        //Drivetrain Direction Toggle
        gamepad1.DIAMOND_LEFT.whenPressed(new ToggleDrivetrainDirectionCommand());
        //Conveyor
        gamepad1.LEFT_TRIGGER.whenPressed(new SetConveyorCommand(Conveyor.CONVEYOR_COLLECT_IN));
        gamepad1.LEFT_TRIGGER.whenReleased(new SetConveyorCommand(Conveyor.CONVEYOR_STOP));
        gamepad1.LEFT_BUMPER.whenPressed(new SetConveyorCommand(Conveyor.CONVEYOR_COLLECT_OUT));
        gamepad1.LEFT_BUMPER.whenReleased(new SetConveyorCommand(Conveyor.CONVEYOR_STOP));
        //Lifter
        gamepad1.DPAD_UP.whenPressed(new SetLifterCommand(Lifter.LifterDirection.LIFTER_UP));
        gamepad1.DPAD_UP.whenReleased(new SetLifterCommand(Lifter.LifterDirection.LIFTER_STOP));
        gamepad1.DPAD_DOWN.whenPressed(new SetLifterCommand(Lifter.LifterDirection.LIFTER_DOWN));
        gamepad1.DPAD_DOWN.whenReleased(new SetLifterCommand(Lifter.LifterDirection.LIFTER_STOP));
        
        gamepad2 = new Gamepad(2);
        //Conveyor
        gamepad2.LEFT_TRIGGER.whenPressed(new SetConveyorCommand(Conveyor.CONVEYOR_SHOOT_IN));
        gamepad2.LEFT_TRIGGER.whenReleased(new SetConveyorCommand(Conveyor.CONVEYOR_STOP));
        gamepad2.LEFT_BUMPER.whenPressed(new SetConveyorCommand(Conveyor.CONVEYOR_SHOOT_OUT));
        gamepad2.LEFT_BUMPER.whenReleased(new SetConveyorCommand(Conveyor.CONVEYOR_STOP));
        //Shooter
        gamepad2.RIGHT_TRIGGER.whenPressed(new SetShooterCommand(Shooter.SHOOTER_ON));
        gamepad2.RIGHT_TRIGGER.whenReleased(new SetShooterCommand(Shooter.SHOOTER_OFF));
        //Augers
        gamepad2.RIGHT_BUMPER.whenPressed(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        gamepad2.MIDDLE_RIGHT.whenPressed(new AugerRotateCommand(Auger.AugerDirection.AUGER_UP));
        gamepad2.MIDDLE_LEFT.whenPressed(new SetAugerCommand(Auger.AugerDirection.AUGER_STOP));
        //Vision Tracking
        gamepad2.DIAMOND_UP.whileHeld(new FixedPointVisionTrackingCommand(FixedPointVisionTrackingCommand.PYRAMID_BACK_MIDDLE_TUNNEL, VisionTrackingCommand.HIGH_GOAL));
        gamepad2.DIAMOND_DOWN.whileHeld(new FixedPointVisionTrackingCommand(FixedPointVisionTrackingCommand.PYRAMID_BACK_MIDDLE_TUNNEL, VisionTrackingCommand.MIDDLE_GOAL));
        //Lifter
        gamepad2.DPAD_UP.whenPressed(new SetLifterCommand(Lifter.LifterDirection.LIFTER_UP));
        gamepad2.DPAD_UP.whenReleased(new SetLifterCommand(Lifter.LifterDirection.LIFTER_STOP));
        gamepad2.DPAD_DOWN.whenPressed(new SetLifterCommand(Lifter.LifterDirection.LIFTER_DOWN));
        gamepad2.DPAD_DOWN.whenReleased(new SetLifterCommand(Lifter.LifterDirection.LIFTER_STOP));
    }
    
    /**
     * Returns the proper instance of Controls.
     * This method creates a new Controls object the first time it is called
     * and returns that object for each subsequent call.
     * @return The current instance of Controls.
     */
    public static Controls getInstance(){
        if(instance == null){
            instance = new Controls();
        }
        return instance;
    }
}

@@ -903,32 +903,37 @@
 ;%0A        //
-Conveyor
+Trigger Drive
 %0A        gam
@@ -930,35 +930,36 @@
        gamepad1.
-LEF
+RIGH
 T_TRIGGER.whenPr
@@ -950,32 +950,30 @@
 T_TRIGGER.wh
-enPresse
+ileHel
 d(new SetCon
@@ -973,52 +973,57 @@
  Set
-ConveyorCommand(Conveyor.CONVEYOR_COLLECT_IN
+DrivetrainStrafeCommand(ONE_JOYSTICK_MAGNITUDE, 0
 ));%0A
@@ -1039,19 +1039,20 @@
 amepad1.
-LEF
+RIGH
 T_TRIGGE
@@ -1077,143 +1077,138 @@
  Set
-ConveyorCommand(Conveyor.CONVEYOR_STOP));%0A        gamepad1.LEFT_BUMPER.whenPressed(new SetConveyorCommand(Conveyor.CONVEYOR_COLLECT_OUT
+DrivetrainStrafeCommand(0, 0));%0A        gamepad1.LEFT_TRIGGER.whileHeld(new SetDrivetrainStrafeCommand(ONE_JOYSTICK_MAGNITUDE, 180
 ));%0A
@@ -1225,28 +1225,29 @@
 mepad1.LEFT_
-BUMP
+TRIGG
 ER.whenRelea
@@ -1261,46 +1261,36 @@
  Set
-ConveyorCommand(Conveyor.CONVEYOR_STOP
+DrivetrainStrafeCommand(0, 0
 ));%0A

Add trigger drive controls to the driver gamepad.