001/************************ PROJECT ZIXI ************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/
003/* This work is licensed under the terms of the MIT license.  */
004/**************************************************************/
005package com.stuypulse.robot.util.talonfx.sim;
006
007import com.stuypulse.robot.util.talonfx.config.TalonFXConfig;
008import com.stuypulse.robot.util.talonfx.logged.LoggedTalonFX;
009
010import org.wpilib.system.RobotController;
011import org.wpilib.units.measure.*;
012
013import com.ctre.phoenix6.CANBus;
014import com.ctre.phoenix6.configs.MotorOutputConfigs;
015import com.ctre.phoenix6.sim.TalonFXSimState;
016
017/**
018 *
019 *
020 * <h2>TalonFX Simulation</h2>
021 *
022 * <p>This is a simulation wrapper for the TalonFX motor controller. It allows for simulating the
023 * behavior of a CTRE motor controller by using WPILib's linear system simulation classes. This
024 * class is designed to be used in a conjunction with a SystemSim instance that abstracts all types
025 * of WPILib linear sims.
026 */
027public class TalonFXSimulation extends LoggedTalonFX {
028    private final SystemSim<?> simMotor;
029    private double gearRatio;
030
031    /**
032     * Creates a new TalonFXSimulation instance.
033     *
034     * @param port The port of the motor controller (TalonFX).
035     * @param gearRatio The gear ratio of the motor.
036     * @param adapter The simulation adapter for the motor.
037     */
038    public TalonFXSimulation(int port, double gearRatio, SystemSim<?> adapter) {
039        super(port, new CANBus("simulation"));
040        this.gearRatio = gearRatio;
041        this.simMotor = adapter;
042    }
043
044    /**
045     * Configures the motor with the given configuration.
046     *
047     * @param config The configuration to apply to the motor.
048     */
049    public void configure(TalonFXConfig config) {
050        config.configure(this);
051    }
052
053    /**
054     * Call once AFTER configuring the motors to link the orientation of this motor to a reference
055     * motor. This ensures that the gear ratio is correctly oriented for the simulation.
056     *
057     * @param reference The reference motor to compensate for orientation. This should be the
058     *     "leader" motor usually.
059     */
060    public void linkToReference(TalonFXSimulation reference) {
061        final MotorOutputConfigs thisConfigs = new MotorOutputConfigs();
062        this.getConfigurator().refresh(thisConfigs);
063
064        final MotorOutputConfigs referenceConfigs = new MotorOutputConfigs();
065        reference.getConfigurator().refresh(referenceConfigs);
066
067        final boolean isSameOrientation = thisConfigs.Inverted == referenceConfigs.Inverted;
068        this.gearRatio = Math.abs(this.gearRatio) * (isSameOrientation ? 1 : -1);
069    }
070
071    /**
072     * Refeshes the simulation state of the motor. This should be called only AFTER updating the
073     * corresponding SystemSim.
074     */
075    public void refresh() {
076        final TalonFXSimState simState = this.getSimState();
077
078        this.simMotor.setInputVoltage(
079                simState.getMotorVoltageMeasure().times(Math.signum(gearRatio)));
080
081        Angle rotorPosition = simMotor.getMechanismPosition().times(this.gearRatio);
082        AngularVelocity rotorVelocity = this.simMotor.getMechanismVelocity().times(this.gearRatio);
083
084        simState.setRawRotorPosition(rotorPosition);
085        simState.setRotorVelocity(rotorVelocity);
086        simState.setSupplyVoltage(RobotController.getBatteryVoltage());
087    }
088}