import wpilib
import wpilib.drive
import ctre

class MyRobot(wpilib.TimedRobot):
    def robotInit(self):
        self.leftFront = ctre.WPI_VictorSPX(0)
        self.leftRear = ctre.WPI_VictorSPX(1)
        self.rightFront = ctre.WPI_VictorSPX(2)
        self.rightRear = ctre.WPI_VictorSPX(3)

        self.left = wpilib.MotorControllerGroup(self.leftFront, self.leftRear)
        self.right = wpilib.MotorControllerGroup(self.rightFront, self.rightRear)
        self.right.setInverted(True)

        self.drivetrain = wpilib.drive.DifferentialDrive(self.left, self.right)

        self.primary = wpilib.XboxController(0)

        self.compressor = wpilib.Compressor(0, wpilib.PneumaticsModuleType.REVPH)
        # self.compressor.enableDigital()
        self.compressor.enableAnalog(75, 115)

        self.singleActionMechanism = wpilib.Solenoid(0, wpilib.PneumaticsModuleType.REVPH, 0)
        self.doubleActionMechanism = wpilib.DoubleSolenoid(0, wpilib.PneumaticsModuleType.REVPH, 1, 2)

    def autonomousInit(self):
        pass

    def autonomousPeriodic(self):
        pass

    def teleopInit(self):
        pass

    def teleopPeriodic(self):
        self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX())

        if self.primary.getAButtonPressed():
            self.singleActionMechanism.set(not self.singleActionMechanism.get())

        if self.primary.getBButtonPressed():
            if self.doubleActionMechanism.get() == wpilib.DoubleSolenoid.Value.kForward:
                self.doubleActionMechanism.set(wpilib.DoubleSolenoid.Value.kReverse)
            elif self.doubleActionMechanism.get() == wpilib.DoubleSolenoid.Value.kReverse:
                self.doubleActionMechanism.set(wpilib.DoubleSolenoid.Value.kForward)
            else:
                self.doubleActionMechanism.set(wpilib.DoubleSolenoid.Value.kReverse)

if __name__ == "__main__":
    wpilib.run(MyRobot)