#
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
#

import wpilib

from constants import IntakeConstants


class Intake:
    def __init__(self) -> None:
        self.motor = wpilib.PWMSparkMax(IntakeConstants.MOTOR_PORT)
        self.piston = wpilib.DoubleSolenoid(
            0,
            wpilib.PneumaticsModuleType.CTRE_PCM,
            IntakeConstants.PISTON_FWD_CHANNEL,
            IntakeConstants.PISTON_REV_CHANNEL,
        )

    def deploy(self) -> None:
        self.piston.setThrottle(wpilib.DoubleSolenoid.Value.FORWARD)

    def retract(self) -> None:
        self.piston.setThrottle(wpilib.DoubleSolenoid.Value.REVERSE)
        self.motor.setThrottle(0)  # turn off the motor

    def activate(self, velocity: float) -> None:
        if self.isDeployed():
            self.motor.setThrottle(velocity)
        else:  # if piston isn't open, do nothing
            self.motor.setThrottle(0)

    def isDeployed(self) -> bool:
        return self.piston.get() == wpilib.DoubleSolenoid.Value.FORWARD