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

#include "subsystems/HatchSubsystem.hpp"

#include "wpi/util/sendable/SendableBuilder.hpp"

using namespace HatchConstants;

HatchSubsystem::HatchSubsystem()
    : hatchSolenoid{0, wpi::PneumaticsModuleType::CTRE_PCM,
                    kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}

wpi::cmd::CommandPtr HatchSubsystem::GrabHatchCommand() {
  // implicitly require `this`
  return this->RunOnce(
      [this] { hatchSolenoid.Set(wpi::DoubleSolenoid::FORWARD); });
}

wpi::cmd::CommandPtr HatchSubsystem::ReleaseHatchCommand() {
  // implicitly require `this`
  return this->RunOnce(
      [this] { hatchSolenoid.Set(wpi::DoubleSolenoid::REVERSE); });
}

void HatchSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
  SubsystemBase::InitSendable(builder);

  // Publish the solenoid state to telemetry.
  builder.AddBooleanProperty(
      "extended",
      [this] { return hatchSolenoid.Get() == wpi::DoubleSolenoid::FORWARD; },
      nullptr);
}
