2026KPopRobotHunters/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java

package frc4388.robot.subsystems.intake;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;

import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.SparkMax;

import edu.wpi.first.units.measure.Angle;
import frc4388.utility.compute.JankCoder;

public class IntakeReal implements IntakeIO {

    SparkMax m_armMotor;
    TalonFX m_rollerMotor;
    JankCoder m_encoder;

    public IntakeReal(
        SparkMax armMotor,
        TalonFX rollerMotor,
        JankCoder jankCoder
    ) {
        // m_angleMotor = angleMotor;
        // m_pitchMotor = pitchMotor;
        m_armMotor = armMotor;
        m_rollerMotor = rollerMotor;
        m_encoder = jankCoder;

        m_armMotor.configure(IntakeConstants.ARM_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
        m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
    }


    
    @Override
    public void setRollerOutput(IntakeState state, double rollerOutput) {
        state.rollerTargetOutput = rollerOutput;

        m_rollerMotor.set(rollerOutput);
    }

    @Override
    public void setArmAngle(IntakeState state, Angle angle) {
        
        state.armTargetAngle = angle;
        // Assume that the angle is always accurate, because I think we will use a shaft encoder
        // Assume that 0 degrees = forwards. Might need an offset here

        // angle = clampAng(angle, IntakeConstants.ARM_LIMIT_RETRACTED, IntakeConstants.ARM_LIMIT_EXTENDED);

        // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
        Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
        
        // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
        // m_armMotor.setControl(
        //     armPosition
        //         .withPosition(motorAngle)
        //         .withLimitReverseMotion(!m_armLimitSwitch.get())
        //     );

    }

    @Override
    public void stopArm(){
        m_armMotor.set(0);
        // m_rollerMotor.set(0);
    }

    private boolean retractedLimit() {
        return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get();
    }
    private boolean extendedLimit() {
        return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get();
    }

    @Override
    public void armOutput(double percentOutput){

        // if(retractedLimit()) {
        //     percentOutput = Math.max(percentOutput, 0);
        // } 
        
        if (extendedLimit()) {
            percentOutput = Math.min(percentOutput, 0);
        }

        m_armMotor.set(percentOutput);

    }

    @Override
    public void updateInputs(IntakeState state) {
        m_encoder.update();


        state.armAngle = Rotations.of(m_armMotor.getEncoder().getPosition()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
        state.armMotorVelocity = RotationsPerSecond.of(m_armMotor.getEncoder().getVelocity()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
        // state.armMotorAcceleration = RotationsPerSecondPerSecond.of(m_armMotor.getEncoder().ge);
        state.armMotorCurrent = Amps.of(m_armMotor.getOutputCurrent());

        state.rollerOutput = m_rollerMotor.get();
        state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();

        state.retractedSoftLimit = retractedLimit();
        state.extendedSoftLimit = extendedLimit();

        state.intakeEncoder = m_encoder.getRotations();
        state.encoderConnected = m_encoder.isConnected();

        state.retractedLimitSwitch = m_armMotor.getReverseLimitSwitch().isPressed();

        if(state.retractedLimitSwitch) {
            m_encoder.resetRotations();
        }
    }

    @Override 
    public void updateGains() {
        m_encoder.loadRotations();

        // IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get();
        // IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get();
        // IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get();
        // m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);

    }
}