package frc.robot.subsystems;

import org.littletonrobotics.junction.Logger;

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

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

public class Hooks extends SubsystemBase 
{
    // initial angle 237
    private final CANSparkMax m_hookL = new CANSparkMax(Constants.hookL_ID, MotorType.kBrushed);
    private final CANSparkMax m_hookR = new CANSparkMax(Constants.hookR_ID, MotorType.kBrushed);

    private final Encoder m_hookL_encoder = new Encoder(Constants.hookL_PINS[0], Constants.hookL_PINS[1]);
    private final Encoder m_hookR_encoder = new Encoder(Constants.hookR_PINS[0], Constants.hookR_PINS[1]);
    
    public Hooks() 
    { 
        // TODO: possibly invert to move forward upon positive votage
        m_hookL.setInverted(true);
        m_hookL_encoder.setReverseDirection(true);
    }

    public void atPercentage(double percL, double percR)
    {
        m_hookL.set(percL);
        m_hookR.set(percR);
    }

    @Override
    public void periodic()
    {
        Logger.recordOutput(getName() + "/encoderL", m_hookL_encoder.getDistance());
        Logger.recordOutput(getName() + "/encoderR", m_hookR_encoder.getDistance());
    }
}