001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.util.shooter;
007
008import com.stuypulse.robot.constants.Field;
009import com.stuypulse.robot.constants.Settings;
010import com.stuypulse.robot.constants.Settings.Shooter.FerryRPMInterpolation;
011import com.stuypulse.robot.constants.Settings.Shooter.FerryTOFInterpolation;
012import com.stuypulse.robot.constants.Settings.Shooter.RPMInterpolation;
013import com.stuypulse.robot.constants.Settings.Shooter.TOFInterpolation;
014import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
015import static edu.wpi.first.units.Units.RPM;
016import dev.doglog.DogLog;
017import edu.wpi.first.math.geometry.Pose2d;
018import edu.wpi.first.math.geometry.Translation2d;
019import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
020
021public class InterpolationCalculator {
022
023    public static InterpolatingDoubleTreeMap shootingDistanceRPMInterpolator;
024
025    public static InterpolatingDoubleTreeMap shootingDistanceTOFInterpolator;
026
027    public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator;
028
029    public static InterpolatingDoubleTreeMap ferryingDistanceTOFInterpolator;
030
031    public record InterpolatedInfo(double targetRPM, double flightTimeSeconds) {
032    }
033
034    static {
035        shootingDistanceRPMInterpolator = new InterpolatingDoubleTreeMap();
036        for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) {
037            shootingDistanceRPMInterpolator.put(pair[0], pair[1]);
038        }
039        shootingDistanceTOFInterpolator = new InterpolatingDoubleTreeMap();
040        for (double[] pair : TOFInterpolation.distanceTOFInterpolationValues) {
041            shootingDistanceTOFInterpolator.put(pair[0], pair[1]);
042        }
043        ferryingDistanceRPMInterpolator = new InterpolatingDoubleTreeMap();
044        for (double[] pair : FerryRPMInterpolation.ferryDistanceRPMInterpolation) {
045            ferryingDistanceRPMInterpolator.put(pair[0], pair[1]);
046        }
047        ferryingDistanceTOFInterpolator = new InterpolatingDoubleTreeMap();
048        for (double[] pair : FerryTOFInterpolation.FerryTOFInterpolationInterpolation) {
049            ferryingDistanceTOFInterpolator.put(pair[0], pair[1]);
050        }
051    }
052
053    public static InterpolatedInfo interpolateShotInfo() {
054        CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance();
055        return interpolateShotInfo(swerve.getPose(), Field.getHubPose());
056    }
057
058    public static InterpolatedInfo interpolateShotInfo(Pose2d shooterPose, Pose2d targetPose) {
059        Translation2d hubPose = targetPose.getTranslation();
060        Translation2d currentPose = shooterPose.getTranslation();
061        double distanceMeters = currentPose.getDistance(hubPose);
062        double targetRPM = shootingDistanceRPMInterpolator.get(distanceMeters);
063        double flightTime = shootingDistanceTOFInterpolator.get(distanceMeters);
064        DogLog.log("InterpolationTesting/Interpolated RPM", targetRPM);
065        DogLog.log("InterpolationTesting/Interpolated TOF", flightTime);
066        // if (targetRPM < Settings.Shooter.MIN_SHOOTER_VELOCITY.in(RPM))
067        //     targetRPM = Settings.Shooter.MIN_SHOOTER_VELOCITY.in(RPM);
068        return new InterpolatedInfo(targetRPM, flightTime);
069    }
070
071    public static InterpolatedInfo interpolateFerryingInfo() {
072        CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance();
073        Pose2d shooterPose = swerve.getPose();
074        Pose2d ferryPose = Field.getFerryZonePose(swerve.getPose().getTranslation());
075        return interpolateFerryingInfo(shooterPose, ferryPose);
076    }
077
078    public static InterpolatedInfo interpolateFerryingInfo(Pose2d shooterPose, Pose2d targetPose) {
079        Translation2d currentPose = shooterPose.getTranslation();
080        Translation2d ferryPose = targetPose.getTranslation();
081        double distanceMeters = currentPose.getDistance(ferryPose);
082        double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters);
083        double flightTime = ferryingDistanceTOFInterpolator.get(distanceMeters);
084        DogLog.log("Shooter/Interpolated Ferry RPM", targetRPM);
085        DogLog.log("Shooter/Interpolated Ferry TOF", flightTime);
086        return new InterpolatedInfo(targetRPM, flightTime);
087    }
088}