package first.robot.subsystems;

import java.util.Optional;
import java.util.function.DoubleSupplier;

import org.wpilib.command2.Command;
import org.wpilib.command2.SubsystemBase;
import org.wpilib.epilogue.Logged;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;

import com.revrobotics.PersistMode;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkLowLevel.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;

import first.robot.constants.HoodConstants;
import first.robot.constants.ShooterConstants.ShooterSpeeds;
import first.robot.utilities.Utilities;

@Logged
public class Hood extends SubsystemBase {
    private SparkMax motor;

    private RelativeEncoder encoder;

    private SparkClosedLoopController controller;

    private double currentTargetRadians;

    public Hood() {
        motor = new SparkMax(
            HoodConstants.kCANPort, 
            HoodConstants.kMotorCANID, 
            MotorType.kBrushless
        );

        motor.configure(
            HoodConstants.kConfig, 
            ResetMode.kResetSafeParameters, 
            PersistMode.kPersistParameters
        );   

        encoder = motor.getEncoder();
        encoder.setPosition(HoodConstants.kStartupAngle);

        controller = motor.getClosedLoopController();

        currentTargetRadians = HoodConstants.kStartupAngle;
    }

    public double getHoodTargetDegrees() {
        return Math.toDegrees(currentTargetRadians);
    }

    public double getHoodTargetRadians() {
        return currentTargetRadians;
    }

    public double getCurrentHoodAngleDegrees() {
        return Math.toDegrees(encoder.getPosition().get());
    }

    public double getCurrentHoodAngleRadians() {
        return encoder.getPosition().get();
    }

    public double getOutputVoltage() {
        return motor.getAppliedOutput().get() * motor.getBusVoltage().get();
    }

    public double getOutputCurrent() {
        return motor.getOutputCurrent().get();
    }

    public boolean isHoodAtSetpoint() {
        return controller.isAtSetpoint().get();
    }

    public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
        return trackToAngle(() -> {
            Pose2d drivetrainPose = drivetrain.getPose();
            Pose2d hubPose = Utilities.getHubPose();

            double distance = drivetrainPose.getTranslation()
                .getDistance(hubPose.getTranslation());

            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
                
            if(currentSpeeds.isPresent()) {
                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());

                if(map != null) {
                    return Math.clamp(map.get(distance), 0, 40);
                } else {
                    return 0;
                }
            } else {
                return 0;
            }
        });
    }

    public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
        return run(() -> {
            currentTargetRadians = radianAngleSupplier.getAsDouble();

            controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
        });
    }

    public Command manualSpeed(DoubleSupplier speed) {
        currentTargetRadians = 0;

        return run(() -> {
            motor.setThrottle(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
        });
    }

    public Command stop() {
        return manualSpeed(() -> 0);
    }
}