import com.ctre.phoenix6.configs.*;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.GravityTypeValue;

public class ArmSubsystem extends SubsystemBase {

    private final TalonFX m_motor = new TalonFX(ArmConstants.MOTOR_CAN_ID, "rio");

    // Reusable control request — allocate once, reuse every loop
    private final MotionMagicVoltage m_mmRequest = new MotionMagicVoltage(0);

    public ArmSubsystem() {
        TalonFXConfiguration cfg = new TalonFXConfiguration();

        // ── Sensor ratio ──────────────────────────────────────────────
        // Tell the TalonFX the gear ratio so position reads in arm rotations
        // (not motor shaft rotations). 1.0 arm rotation = ratio motor rotations.
        cfg.Feedback.withSensorToMechanismRatio(ArmConstants.GEAR_RATIO);

        // ── Motion Magic constraints ───────────────────────────────────
        cfg.MotionMagic
            .withMotionMagicCruiseVelocity(0.5)   // arm rot/s at mechanism
            .withMotionMagicAcceleration(1.0)     // arm rot/s² at mechanism
            .withMotionMagicJerk(10.0);           // optional smoothing; 0 = disabled

        // ── Slot 0 gains ───────────────────────────────────────────────
        // These are the feedback + feedforward gains used during the profile.
        // GravityType.Arm_Cosine makes kG scale with cos(angle) automatically.
        cfg.Slot0
            .withKP(24.0)
            .withKI(0.0)
            .withKD(0.5)
            .withKS(0.25)
            .withKV(0.12)
            .withKA(0.01)
            .withKG(0.35)
            .withGravityType(GravityTypeValue.Arm_Cosine);

        // ── Apply to hardware ──────────────────────────────────────────
        // applyConfiguration() flashes these values to the TalonFX over CAN.
        // In production code, check the StatusCode return value.
        m_motor.getConfigurator().apply(cfg);
    }

    // Call this from a Command's initialize() or execute()
    public void setGoalRotations(double armRotations) {
        m_motor.setControl(m_mmRequest.withPosition(armRotations));
    }

    public boolean atGoal() {
        // ClosedLoopError is in mechanism units after ratio scaling
        double error = m_motor.getClosedLoopError().getValueAsDouble();
        return Math.abs(error) < ArmConstants.POSITION_TOLERANCE_ROT;
    }

    public double getPositionRotations() {
        return m_motor.getPosition().getValueAsDouble();
    }

    @Override
    public void periodic() {
        // No profile calculation here — the TalonFX handles it internally.
        // periodic() is used for telemetry and safety checks only.
        SmartDashboard.putNumber("Arm/Position", getPositionRotations());
        SmartDashboard.putNumber("Arm/ClosedLoopError",
            m_motor.getClosedLoopError().getValueAsDouble());
    }
}

public class MoveArmCommand extends Command {

    private final ArmSubsystem m_arm;
    private final double m_targetRotations;

    public MoveArmCommand(ArmSubsystem arm, double targetRotations) {
        m_arm = arm;
        m_targetRotations = targetRotations;
        addRequirements(arm);
    }

    @Override
    public void initialize() {
        m_arm.setGoalRotations(m_targetRotations);
        // The TalonFX starts the Motion Magic profile immediately.
        // No further calls needed in execute() — the motor drives itself.
    }

    @Override
    public boolean isFinished() {
        return m_arm.atGoal();
    }

    @Override
    public void end(boolean interrupted) {
        if (interrupted) {
            // Hold current position — send a new Motion Magic goal at current pos
            m_arm.setGoalRotations(m_arm.getPositionRotations());
        }
    }
}