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.Intake;
import frc.robot.Constants.Lazer;
import frc.robot.ShuffleboardDisplay;
import com.revrobotics.spark.config.SparkMaxConfig;

import au.grapplerobotics.ConfigurationFailedException;
import au.grapplerobotics.LaserCan;
import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;


public class IntakeSubsystem extends SubsystemBase {
    private final SparkMax Intake_Neo1 = new SparkMax(
        Intake.Port_Neo_Intake,
        Intake.NI_MOTOR_TYPE
    );
    private final SparkMax Intake_Neo2 = new SparkMax(
        Intake.Port_Neo_Intake2,
        Intake.NI_MOTOR_TYPE2
    );
private LaserCan lazer = new LaserCan(Lazer.Lazer);

        private RelativeEncoder encoder;
        private final GenericEntry IntakePosition;
        private final GenericEntry IntakeVelocity;
        private final GenericEntry IntakePosition2;
        private final GenericEntry IntakeVelocity2;
        private final GenericEntry Measurement;
        private Measurement lCan;
        private double position;
        // private double velocity2;
        // private double position2;
        private double velocity;
        private double measurement;
        private SparkMaxConfig IntakeConfig = new SparkMaxConfig();

        
    
        public IntakeSubsystem() {
            try {
                lazer.setRangingMode(LaserCan.RangingMode.SHORT);
                lazer.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_100MS);
                lazer.setRegionOfInterest(new LaserCan.RegionOfInterest(16,16,8,8));
            } catch (ConfigurationFailedException e) {
                e.printStackTrace();
            }
            this.IntakePosition = ShuffleboardDisplay.getInstance().getIntakePosition();
            this.IntakeVelocity = ShuffleboardDisplay.getInstance().getIntakeVelocity();
            this.IntakePosition2 = ShuffleboardDisplay.getInstance().getIntakePosition();
            this.IntakeVelocity2 = ShuffleboardDisplay.getInstance().getIntakeVelocity();
            this.Measurement = ShuffleboardDisplay.getInstance().getMeasurement();
    
            this.IntakeConfig.inverted(true);
            this.Intake_Neo1.configure(IntakeConfig, null, null);
            this.encoder = this.Intake_Neo1.getEncoder();
            
            this.Intake_Neo2.setInverted( Intake.Invert_Neo_Intake);
            this.encoder = this.Intake_Neo2.getEncoder();
    
            this.IntakeConfig.inverted(true);
            this.Intake_Neo1.configure(IntakeConfig, null, null);
            this.encoder = this.Intake_Neo1.getEncoder();
            
            this.Intake_Neo2.setInverted( Intake.Invert_Neo_Intake);
            this.encoder = this.Intake_Neo2.getEncoder();

            this.lCan = this.lazer.getMeasurement();
  
    }
    public void in () {
        
        this.Intake_Neo1.set(Intake.Intake_Neo_in_Speed);
        this.Intake_Neo2.set(Intake.Intake_Neo_in_Speed);
        
    }
    public void secret() {
        this.Intake_Neo1.set(Intake.I1);
        this.Intake_Neo2.set(Intake.I1);
    }
    public void out () {
        this.Intake_Neo1.set(Intake.Intake_Neo_out_Speed);
        this.Intake_Neo2.set(Intake.Intake_Neo_out_Speed);
    }
    public void stop () {
        this.Intake_Neo1.stopMotor();
        this.Intake_Neo2.stopMotor();
    }
    public void CoralTrue(){
        LaserCan.Measurement measurement = lazer.getMeasurement();
        if (measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm <= 30){
            this.Intake_Neo1.stopMotor();
            this.Intake_Neo2.stopMotor();  
        } else {
            this.Intake_Neo1.set(Intake.Intake_Neo_in_Speed);
            this.Intake_Neo2.set(Intake.Intake_Neo_in_Speed);
        }
    }
    public double getVelocity() {
        return this.velocity;
    }
    public double getPosition() {
        return this.position;
    }
    public double getMeasurement(){
        return this.measurement;
    }
}