package frc.robot.subsystems;

import org.littletonrobotics.junction.Logger;

import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;

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

public class Arm extends SubsystemBase 
{
    private final CANSparkMax m_armL = Constants.armL;
    private final CANSparkMax m_armR = Constants.armR;

    private final RelativeEncoder m_encoderL = m_armL.getEncoder();
    private final RelativeEncoder m_encoderR = m_armR.getEncoder();

    private final ArmFeedforward m_feedforward = Constants.arm_feedforward;

    private final ProfiledPIDController m_pidL = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
    private final ProfiledPIDController m_pidR = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
    
    public Arm()
    {   
        m_armR.setInverted(true);

        m_encoderL.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
        m_encoderR.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
    }

    // in rad
    public void atAngle(double angle)
    {
        voltage(
            m_pidL.calculate(m_encoderL.getPosition())
            + m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidR.getSetpoint().velocity),

            m_pidR.calculate(m_encoderR.getPosition())
            + m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
        );
    }


    public void voltage(double leftVolts, double rightVolts)
    {
        m_armL.setVoltage(leftVolts);
        m_armR.setVoltage(rightVolts);
    }

    @Override
    public void periodic() 
    {
        Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition());
        Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition());
    }
}