package frc.robot.subsystems;

import org.littletonrobotics.junction.Logger;

import com.revrobotics.CANSparkMax;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class Chain extends SubsystemBase 
{
    private final CANSparkMax m_motor = Constants.chain;
    private final Encoder m_encoder = Constants.chain_encoder;

    private final ProfiledPIDController m_pid = new ProfiledPIDController(Constants.ChainkP, Constants.ChainkI, Constants.ChainkD, Constants.chain_constraints);

    public Chain()
    {
        // m_motor.setInverted(true);
        // m_encoder.setReverseDirection(true);

        m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
        m_encoder.reset();
    }

    public void reset() 
    {
        m_encoder.reset();
    }

    // perc between 0 and 1;
    public void atPercentage(double perc) 
    {
        m_motor.setVoltage(m_pid.calculate(m_encoder.getDistance(), perc));
    }

    @Override
    public void periodic() 
    { 
        Logger.recordOutput(getName() + "/position", m_encoder.getDistance());
    }
}