package frc.robot.subsystems;

import org.littletonrobotics.junction.Logger;

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

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class Intake extends SubsystemBase 
{
    private final CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
    private final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
    private final RelativeEncoder m_intakeD_encoder = m_intakeD.getEncoder();

    public Intake() 
    {
        m_intakeT.setInverted(true);
        m_intakeD.setInverted(true);
    }

    public void atPercentage(double perc) 
    {
        atPercentage(perc, perc); 
    }

    // perc is between 0.0 and 1.0
    public void atPercentage(double upPerc, double downPerc) 
    {
        m_intakeT.set(upPerc);
        m_intakeD.set(downPerc);
    }

    public void shoot() 
    {
        if (Math.abs(m_intakeD_encoder.getVelocity()) > 2000) {
            m_intakeT.set(-0.3);
        }
        m_intakeD.set(-0.3);
    }

    @Override
    public void periodic()
    {
        Logger.recordOutput(getName() + "/speedT", m_intakeT.getEncoder().getVelocity());
        Logger.recordOutput(getName() + "/speedD", m_intakeD.getEncoder().getVelocity());
    }
}