package frc.robot.subsystems;

import org.littletonrobotics.junction.Logger;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;

import edu.wpi.first.math.MathUtil;
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 = new CANSparkMax(Constants.chain_ID, MotorType.kBrushed);
    private final Encoder m_encoder = new Encoder(Constants.chain_encoder_PINS[0], Constants.chain_encoder_PINS[1]);

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

    public Chain()
    {
        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) 
    {
        Logger.recordOutput(getName() + "/perc", perc);
        double pid_out = m_pid.calculate(m_encoder.getDistance(), perc);

        Logger.recordOutput(getName() + "/pidOUt", pid_out);
        m_motor.setVoltage(MathUtil.clamp(pid_out, -11, 11));
    }

    public boolean isAtPosition(double pos)
    {
        if(Math.abs(pos - m_encoder.getDistance()) < Constants.ChainEpsilon)
            return true;
        return false;
    }

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