// 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.

package org.wpilib.examples.rapidreactcommandbot.subsystems;

import org.wpilib.command2.Command;
import org.wpilib.command2.SubsystemBase;
import org.wpilib.epilogue.Logged;
import org.wpilib.hardware.pneumatic.Compressor;
import org.wpilib.hardware.pneumatic.PneumaticsModuleType;
import org.wpilib.hardware.rotation.AnalogPotentiometer;

/** Subsystem for managing the compressor, pressure sensor, etc. */
@Logged
public class Pneumatics extends SubsystemBase {
  // External analog pressure sensor
  // product-specific voltage->pressure conversion, see product manual
  // in this case, 250(V/5)-25
  // the scale parameter in the AnalogPotentiometer constructor is scaled from 1 instead of 5,
  // so if r is the raw AnalogPotentiometer output, the pressure is 250r-25
  static final double kScale = 250;
  static final double kOffset = -25;
  private final AnalogPotentiometer pressureTransducer =
      new AnalogPotentiometer(/* the AnalogIn port*/ 2, kScale, kOffset);

  // Compressor connected to a PCM with a default CAN ID (0)
  private final Compressor compressor = new Compressor(0, PneumaticsModuleType.CTRE_PCM);

  /**
   * Query the analog pressure sensor.
   *
   * @return the measured pressure, in PSI
   */
  public double getPressure() {
    // Get the pressure (in PSI) from an analog pressure sensor connected to the RIO.
    return pressureTransducer.get();
  }

  /**
   * Disable the compressor closed-loop for as long as the command runs.
   *
   * <p>Structured this way as the compressor is enabled by default.
   *
   * @return command
   */
  public Command disableCompressorCommand() {
    return startEnd(
            // Disable closed-loop mode on the compressor.
            compressor::disable,
            // Enable closed-loop mode based on the digital pressure switch connected to the
            // PCM/PH.
            // The switch is open when the pressure is over ~120 PSI.
            compressor::enableDigital)
        .withName("Compressor Disabled");
  }
}
