HubTracker.java

package frc.robot.util;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.config.RobotConstants;
import frc.robot.robotcontainer.enums.MatchPeriod;
import java.util.Optional;

public class HubTracker {

  private static Optional<Alliance> generatedAutoWinner = Optional.empty();
  private static Optional<Alliance> autoWinner = Optional.empty();

  public static double getCurrentMatchTime() {
    return DriverStation.getMatchTime();
  }

  public static boolean isBlue() {
    return DriverStation.getAlliance().equals(Optional.of(Alliance.Blue));
  }

  public static boolean isAuton() {
    return DriverStation.isAutonomous();
  }

  public static boolean isEnableShoot() {
    return currentShiftIsOurs() || timeLeftInShiftSeconds() < 3.0;
  }

  public static MatchPeriod getMatchPeriod() {
    if (getCurrentMatchTime() < 20 && getCurrentMatchTime() >= 0 && isAuton()) {
      return MatchPeriod.AUTO;
    } else if (getCurrentMatchTime() < 140 && getCurrentMatchTime() >= 130) {
      return MatchPeriod.TRANSITION;
    } else if (getCurrentMatchTime() < 130 && getCurrentMatchTime() >= 105) {
      return MatchPeriod.SHIFT_ONE;
    } else if (getCurrentMatchTime() < 105 && getCurrentMatchTime() >= 80) {
      return MatchPeriod.SHIFT_TWO;
    } else if (getCurrentMatchTime() < 80 && getCurrentMatchTime() >= 55) {
      return MatchPeriod.SHIFT_THREE;
    } else if (getCurrentMatchTime() < 55 && getCurrentMatchTime() >= 30) {
      return MatchPeriod.SHIFT_FOUR;
    } else if (getCurrentMatchTime() < 30 && getCurrentMatchTime() >= 0) {
      return MatchPeriod.ENDGAME;
    } else {
      return MatchPeriod.NONE;
    }
  }

  public static Optional<Alliance> getAutoWinner() {
    if (!RobotConstants.isCompRobot) {
      if (getMatchPeriod() == MatchPeriod.AUTO || getMatchPeriod() == MatchPeriod.NONE) {
        return Optional.empty();
      }
      if (generatedAutoWinner.isEmpty()) {
        resetGeneratedAutoWinner();
      }
      return generatedAutoWinner;
    }

    if (!autoWinner.isEmpty()) {
      return autoWinner;
    }

    String gameData = DriverStation.getGameSpecificMessage();
    if (gameData.length() > 0) {
      switch (gameData.charAt(0)) {
        case 'B':
          autoWinner = Optional.of(Alliance.Blue);
          break;
        case 'R':
          autoWinner = Optional.of(Alliance.Red);
          break;
        default:
          System.err.println("THIS MESSAGE IS CORRUPT");
          break;
      }
    }

    return autoWinner;
  }

  public static double timeLeftInShiftSeconds() {
    switch (getMatchPeriod()) {
      case AUTO:
        return DriverStation.getMatchTime();
      case TRANSITION:
        if (!weWonAuto()) {
          return DriverStation.getMatchTime() - 105;
        } else {
          return DriverStation.getMatchTime() - 130;
        }
      case SHIFT_ONE:
        return DriverStation.getMatchTime() - 105;
      case SHIFT_TWO:
        return DriverStation.getMatchTime() - 80;
      case SHIFT_THREE:
        return DriverStation.getMatchTime() - 55;
      case SHIFT_FOUR:
        if (weWonAuto()) {
          return DriverStation.getMatchTime();
        } else {
          return DriverStation.getMatchTime() - 30;
        }
      case ENDGAME:
        return DriverStation.getMatchTime();
      default:
        return 0.0;
    }
  }

  public static void resetGeneratedAutoWinner() {
    if (Math.random() < 0.5) {
      generatedAutoWinner = Optional.of(Alliance.Blue);
    } else {
      generatedAutoWinner = Optional.of(Alliance.Red);
    }
  }

  public static boolean weWonAuto() {
    if (DriverStation.getAlliance().isPresent() && getAutoWinner().isPresent()) {
      return getAutoWinner().get().equals(DriverStation.getAlliance().get());
    }

    return false;
  }

  public static Alliance currentShift() {

    Alliance ourAlliance = DriverStation.getAlliance().get();

    if (getAutoWinner().isEmpty()) {
      return ourAlliance;
    }

    Alliance autoWinner = getAutoWinner().get();
    Alliance other = autoWinner == Alliance.Blue ? Alliance.Red : Alliance.Blue;

    MatchPeriod period = getMatchPeriod();
    if (period == MatchPeriod.AUTO
        || period == MatchPeriod.TRANSITION
        || period == MatchPeriod.ENDGAME) {
      return ourAlliance;
    }

    if (period == MatchPeriod.SHIFT_TWO || period == MatchPeriod.SHIFT_FOUR) {
      return autoWinner;
    }

    return other;
  }

  public static boolean currentShiftIsOurs() {

    Optional<Alliance> alliance = DriverStation.getAlliance();
    if (alliance.isEmpty()) {
      return true;
    }
    return currentShift().equals(alliance.get());
  }
}