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 AlgaeSubsystem extends SubsystemBase {
    private final SparkMax AlgaeN = new SparkMax(
        Algae.Algaen,
        Algae.NA_MOTOR_TYPE
    );
   

    private RelativeEncoder encoder;
        private final GenericEntry AlgaePosition;
        private final GenericEntry AlgaeVelocity;
        
        private double position;
        private double velocity;
    
        @SuppressWarnings("deprecation")
        public AlgaeSubsystem() {
            this.AlgaePosition = ShuffleboardDisplay.getInstance().getAlgaePosition();
            this.AlgaeVelocity = ShuffleboardDisplay.getInstance().getAlgaeVelocity();
          
    
            this.AlgaeN.setInverted( Algae.Invert_Algae);
            this.encoder = this.AlgaeN.getEncoder();
    }
    @Override
    public void periodic () {
        this.position = this.encoder.getPosition();
        this.velocity = this.encoder.getVelocity();
        this.AlgaePosition.setDouble(this.position);
        this.AlgaeVelocity.setDouble(this.velocity);
   
    }
    public void in () {
        this.AlgaeN.set(Algae.AlgaeIn);
    }
    public void out () {
        this.AlgaeN.set(Algae.AlgaeOut);
    }
    public void stop () {
        this.AlgaeN.stopMotor();
    }
    public double getVelocity() {
        return this.velocity;
    }
    public double getPosition() {
        return this.position;
    }
}