2022NoWayHome/src/main/java/frc4388/robot/subsystems/BoomBoom.java

// 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 frc4388.robot.subsystems;

import java.io.File;
import java.io.IOException;
import java.util.Comparator;
import java.util.Map;
import java.util.Objects;
import java.util.Optional;
import java.util.function.Function;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.regex.Pattern;
import java.util.stream.IntStream;

import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.diffplug.common.base.Errors;

import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.CSV;
import frc4388.utility.Gains;
import frc4388.utility.NumericData;

public class BoomBoom extends SubsystemBase {
  private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
  public WPI_TalonFX m_shooterFalconLeft;
  public WPI_TalonFX m_shooterFalconRight;
  public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
  public static BoomBoom m_boomBoom;

  double velP;
  double input;

  public boolean m_isDrumReady = false;
  public double m_fireVel;

  public Hood m_hoodSubsystem;
  public Turret m_turretSubsystem;

  // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later

  public static class ShooterTableEntry {
    public Double distance, hoodExt, drumVelocity;
  }

  private ShooterTableEntry[] m_shooterTable;

  /*
  * Creates new BoomBoom subsystem, has drum shooter and angle adjuster
  */
  /** Creates a new BoomBoom. */
  public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
    m_shooterFalconLeft = shooterFalconLeft;
    m_shooterFalconRight = shooterFalconRight;
    m_shooterTable = readShooterTable();
    // Run a helper method that logs the contents of the table on a new thread.
    new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
  }

  public Double getVelocity(Double distance) {
    // This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
    // linear interpolation of the two values (drumVelocity) at the two closest points in the table
    // (m_shooterTable) to the given value (distance).
    return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
  }

  public Double getHood(Double distance) {
    // This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
    // interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
    // to the given value (distance).
    return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }

  public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
    m_hoodSubsystem = subsystem0;
    m_turretSubsystem = subsystem1;
  }

  /**
   * Runs the Drum motor at a given speed
   * @param speed percent output form -1.0 to 1.0
   */
  public void runDrumShooter(double speed) {
    m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);

  }

  public void setShooterGains() {
    m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
  }

  public void runDrumShooterVelocityPID(double targetVel) {
    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
    
    // New BoomBoom controller stuff
    // Controls a motor with the output of the BangBang controller
    // Controls a motor with the output of the BangBang conroller and a feedforward
    // Shrinks the feedforward slightly to avoid over speeding the shooter

    // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 *
    // feedforward.calculate(targetVel));
    // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
  }

  private static ShooterTableEntry[] readShooterTable() {
    try {
      // This is a helper class that allows us to read a CSV file into a Java array.
      CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
        // This is a regular expression that removes all parentheses from the header of the CSV file.
        private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");

        /**
         * Removes the parentheses from the CSV header
         * 
         * @param header The header to be sanitized.
         * @return The headerSanitizer method is overriding the headerSanitizer method in the parent class.
         *         The parentheses.matcher(header) is matching the header with the parentheses regular
         *         expression. The replaceAll method is replacing all of the parentheses with an empty
         *         string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling
         *         the parent sanitizer.
         */
        @Override
        protected String headerSanitizer(String header) {
          return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
        }
      };
      // This is reading the CSV file into a Java array.
      return csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
    } catch (IOException exception) {
      ShooterTableEntry dummyEntry = new ShooterTableEntry();
      dummyEntry.distance = 0.0;
      dummyEntry.hoodExt = 0.0;
      dummyEntry.drumVelocity = 0.0;
      LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
      return new ShooterTableEntry[] { dummyEntry };
    }
  }
}