package frc.robot.subsystems;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

import frc.robot.Constants.Algae;
import frc.robot.ShuffleboardDisplay;


public class AlgaeArmSubsystem extends SubsystemBase {
    private final SparkMax AlgaeNA = new SparkMax(
        Algae.AlgaeNM,
        Algae.NA_MOTOR_TYPE2
    );
   

    private RelativeEncoder encoder;
        private final GenericEntry AlgaeArmPosition;
        private final GenericEntry AlgaeArmVelocity;
        
        private double position;
        private double velocity;
    
        @SuppressWarnings("deprecation")
        public AlgaeArmSubsystem() {
            this.AlgaeArmPosition = ShuffleboardDisplay.getInstance().getAlgaeArmPosition();
            this.AlgaeArmVelocity = ShuffleboardDisplay.getInstance().getAlgaeArmVelocity();
          
    
            this.AlgaeNA.setInverted( Algae.Invert_Neo_AlgaeNM);
            this.encoder = this.AlgaeNA.getEncoder();
    }
    @Override
    public void periodic () {
        this.position = this.encoder.getPosition();
        this.velocity = this.encoder.getVelocity();
        this.AlgaeArmPosition.setDouble(this.position);
        this.AlgaeArmVelocity.setDouble(this.velocity);

    }
    public void up () {
        this.AlgaeNA.set(Algae.AlgaeUp);
    }
    public void down () {
        this.AlgaeNA.set(Algae.AlgaeDown);
    }
    public void stop () {
        this.AlgaeNA.stopMotor();
    }
    public double getVelocity() {
        return this.velocity;
    }
    public double getPosition() {
        return this.position;
    }
    public void setRaw(double value) {
        this.AlgaeNA.set(this.valueBound(value));
       
    }
    private double valueBound(double value) {
        return Math.min(1.0, Math.max(-1.0, value));
    }
}