package frc.robot.subsystems;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.Intake;
import frc.robot.Constants.FieldMiscellaneous;
import frc.robot.Constants;
import frc.robot.util;

import java.util.function.Supplier;

import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


public class IntakeSubsystem extends SubsystemBase {
    private final SparkMax armMotor = new SparkMax(
        Intake.ARMMOTOR_ID,
        MotorType.kBrushless
    );
    private final SparkMax intakeMotor = new SparkMax(
        Intake.INTAKEMOTOR_ID,
        MotorType.kBrushless
    );
   
    private final Supplier<Pose2d> poseSupplier;
    private boolean safe = true;
    private DutyCycleEncoder ArmAbsoluteEncoder;
    private SparkClosedLoopController armMotorClosedLoopController;
    private SparkClosedLoopController intakeMotorClosedLoopController;
    private double absArmPosition;
    private double armPosition;
    private double armVelocity;
    private double intakePosition;
    private double intakeVelocity;
    private final Rectangle2d[] BUMPERSPLUSROBOTRADIUS = util.offsetRectangle2dArray(FieldMiscellaneous.gBUMPS, Constants.ROBOTFULLRADIUS); // make rect2d array that has all of the bumps offset by the radius of the robot
    private final Rectangle2d[] BUMPERSPLUSROBOTRADIUSPLUSHYSTERESIS = util.offsetRectangle2dArray(BUMPERSPLUSROBOTRADIUS, Intake.BUMP_HYSTERESIS); // precomputing this is probably a little bit faster than just iterating over the rectangle.distancetopoint or whatever it is called

    public IntakeSubsystem(Supplier<Pose2d> poseSupplier) {
        this.poseSupplier = poseSupplier;
        this.ArmAbsoluteEncoder = new DutyCycleEncoder(Intake.ENCODER_PORT);
        this.ArmAbsoluteEncoder.setInverted(true);
        armMotorClosedLoopController = armMotor.getClosedLoopController();
        intakeMotorClosedLoopController = intakeMotor.getClosedLoopController();
        armMotor.configure(Intake.ARMMOTORCONFIG, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
        intakeMotor.configure(Intake.INTAKEMOTORCONFIG, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
        this.absArmPosition = (this.ArmAbsoluteEncoder.get()+Intake.ABSOLUTEOFFSET)%1;
        this.armMotor.getEncoder().setPosition(this.absArmPosition%1);
    }
    @Override
    public void periodic () {
        this.absArmPosition = (this.ArmAbsoluteEncoder.get()+Intake.ABSOLUTEOFFSET)%1;
        this.armPosition = this.armMotor.getEncoder().getPosition();
        this.armVelocity = this.armMotor.getEncoder().getVelocity();
        this.intakePosition = this.intakeMotor.getEncoder().getPosition();
        this.intakeVelocity = this.intakeMotor.getEncoder().getVelocity();
        if (Math.abs(this.armPosition - this.absArmPosition)> 0.02) { // MAYBE TRY DISABLING THIS TO FIX THE CAN ERRORS 2/20/2026
            this.armMotor.getEncoder().setPosition(this.absArmPosition);
        }
        if (util.pointInsideRectangle2dArray(BUMPERSPLUSROBOTRADIUS, poseSupplier.get().getTranslation())) { // if robot is ontop of any bumps then it is not safe to deploy the arm
            safe = false;
        }
        if (!util.pointInsideRectangle2dArray(BUMPERSPLUSROBOTRADIUSPLUSHYSTERESIS, poseSupplier.get().getTranslation())) { // if the robot is outside all bumps plus a hysteresis number then it is safe to deploy the arm
            safe = true;
        }
        if(!safe) {
            this.retract(); // retract arm when going over bump lest the intake arm explodes into a million little pieces that we then have to clean up
        }
        SmartDashboard.putNumber("Intake arm absolute angle (turns)", this.absArmPosition); // eight billion smartdashboard things
        SmartDashboard.putNumber("Intake arm relative angle (turns)", this.armPosition);
        SmartDashboard.putNumber("Intake arm velocity (rpm)", this.armVelocity);
        SmartDashboard.putNumber("Intake arm power (-1 to 1)", this.armMotor.get());
        SmartDashboard.putNumber("Intake arm setpoint", this.armMotorClosedLoopController.getSetpoint());
        SmartDashboard.putString("Intake arm setpoint units", this.armMotorClosedLoopController.getControlType().toString());
        SmartDashboard.putBoolean("Intake arm at setpoint? (boolean)", this.armMotorClosedLoopController.isAtSetpoint());
        SmartDashboard.putBoolean("Intake arm deployable? (boolean)", safe);

        SmartDashboard.putNumber("Intake feeder relative angle (turns)", this.intakePosition);
        SmartDashboard.putNumber("Intake feeder velocity (rpm)", this.intakeVelocity);
        SmartDashboard.putNumber("Intake feeder power (-1 to 1)", this.intakeMotor.get());
        SmartDashboard.putNumber("Intake feeder setpoint", this.intakeMotorClosedLoopController.getSetpoint());
        SmartDashboard.putString("Intake feeder setpoint units", this.intakeMotorClosedLoopController.getControlType().toString());
        SmartDashboard.putBoolean("Intake feeder at setpoint? (boolean)", this.intakeMotorClosedLoopController.isAtSetpoint());
    }
    public void retract () {
        this.armMotorClosedLoopController.setSetpoint(Intake.SHOOTINGANGLE, ControlType.kPosition);
        this.intakeMotor.set(Intake.INTAKEFEEDERMODESPEED);
    }
    public void deploy () {
        this.armMotorClosedLoopController.setSetpoint(Intake.INTAKEANGLE, ControlType.kPosition);
        this.intakeMotor.set(Intake.INTAKESPEED);
    }
    public void stop () {
        this.armMotor.stopMotor();
        this.intakeMotor.stopMotor();
    }
    public double getArmPosition() {
        return this.armPosition;
    }
    public double getArmVelocity() {
        return this.armVelocity;
    }
    public boolean armAtSetpoint() {
        return this.armMotorClosedLoopController.isAtSetpoint();
    }
    public double getIntakePosition() {
        return this.intakePosition;
    }
    public double getIntakeVelocity() {
        return this.intakeVelocity;
    }
    public boolean intakeAtSetpoint() {
        return this.intakeMotorClosedLoopController.isAtSetpoint();
    }
    public void setArmPosition(double value) {
        this.armMotorClosedLoopController.setSetpoint(MathUtil.clamp(value, Intake.ABSOLUTEMINANGLE, Intake.ABSOLUTEMAXANGLE), ControlType.kPosition);
    }
}