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

import frc.robot.ShuffleboardDisplay;


public class ElevatorSubsystem extends SubsystemBase {
    private final SparkMax Elevator_neo1 = new SparkMax(
        Elevator.Elevator_Neo1,
        Elevator.E_MOTOR_TYPE
    );
    private final SparkMax Elevator_neo2 = new SparkMax(
        Elevator.Elevator_Neo2,
        Elevator.E_MOTOR_TYPE2
    );

    private RelativeEncoder encoder;
    private RelativeEncoder encoder2;
        private final GenericEntry ElevatorPosition;
        private final GenericEntry ElevatorVelocity;
        private final GenericEntry ElevatorPosition2;
        private final GenericEntry ElevatorVelocity2;
        
        private double position;
        private double velocity2;
        private double position2;
        private double velocity;
        private SparkMaxConfig EConfig = new SparkMaxConfig();

        @SuppressWarnings("deprecation")
        public ElevatorSubsystem() {
            this.ElevatorPosition = ShuffleboardDisplay.getInstance().getElevatorPosition();
            this.ElevatorVelocity = ShuffleboardDisplay.getInstance().getElevatorVelocity();
            this.ElevatorPosition2 = ShuffleboardDisplay.getInstance().getElevatorPosition2();
            this.ElevatorVelocity2 = ShuffleboardDisplay.getInstance().getElevatorVelocity2();
    
    
            this.Elevator_neo1.setInverted( Elevator.Invert_Neo_Elevator1);
            this.EConfig.inverted(false);
            this.Elevator_neo2.configure(EConfig, null, null);
            this.encoder = this.Elevator_neo1.getEncoder();
            this.encoder2 = this.Elevator_neo2.getEncoder();
    }
    @Override
    public void periodic () {
        this.position = this.encoder.getPosition();
        this.velocity = this.encoder.getVelocity();
        this.velocity2 = this.encoder2.getVelocity();
        this.position2 = this.encoder2.getPosition();

        this.ElevatorPosition.setDouble(this.position);
        this.ElevatorPosition2.setDouble(this.position2);
        this.ElevatorVelocity2.setDouble(this.velocity2);
        this.ElevatorVelocity.setDouble(this.velocity);
  
    }
    public void in () {
        this.Elevator_neo1.set(Elevator.Elevator_Neo_in_Speed);
        this.Elevator_neo2.set(Elevator.Elevator_Neo_in_Speed2);
    }
    public void out () {
        this.Elevator_neo1.set(Elevator.Elevator_Neo_out_Speed);
        this.Elevator_neo2.set(Elevator.Elevator_Neo_out_Speed2);
    }
    public void stop () {
        this.Elevator_neo1.stopMotor();
        this.Elevator_neo2.stopMotor();
    }
    public void setRaw(double value) {
        this.Elevator_neo1.set(this.valueBound(value));
        this.Elevator_neo2.set(this.valueBound(value));
    }
    public double getVelocity() {
        return this.velocity;
    }
    public double getPosition() {
        return this.position;
    }
    public double getVelocity2() {
        return this.velocity2;
    }
    public double getPosition2() {
        return this.position2;
    }
    private double valueBound(double value) {
        return Math.min(1.0, Math.max(-1.0, value));
    }
    public void zero(){
        this.encoder.setPosition(0);
        this.encoder2.setPosition(0);
    }
}