package frc.robot.subsystems;

import java.util.function.DoubleSupplier;

import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.MotorIDConstants;

public class Elevator extends SubsystemBase {
    //Constraints constants
    private static final double kMaxAcceleration = 0;
    private static final double kMaxVelocity = 0;
    //PID constants
    private static final double kP = 0;
    private static final double kI = 0;
    private static final double kD = 0;
    private static final double kDt = 0.02;
    //Feed Forward constants
    private static final double kS = 0;
    private static final double kG = 0;
    private static final double kV = 0;
    //Position constants
    private static final double kMaxExtension = 0;
    //Homing constants
    private static final double kHomingLocation = 0;
    private static final double kHomingVelocityLimit = 0.01;
    private static final double kHomingDelay = 0.1;
    private static final double kHomingVoltage = -4;

    //----------------------------------------------------------------------------------------------------//

    private double feedForwardOutput;
    private double pidControllerOutput;

    private ElevatorFeedforward feedForward;

    private ProfiledPIDController pidController;

    private RelativeEncoder encoder;

    private SparkMax motor1;

    private TrapezoidProfile.Constraints constraints;

    public Elevator() {

        constraints = new TrapezoidProfile.Constraints(
            kMaxVelocity, 
            kMaxAcceleration
        );

        motor1 = new SparkMax(
            MotorIDConstants.kElevatorMotor1ID, 
            MotorType.kBrushless
        );

        pidController = new ProfiledPIDController(
            kP, 
            kI, 
            kD, 
            constraints, 
            kDt
        );

        feedForward = new ElevatorFeedforward(
            kS, 
            kG, 
            kV
        );
    }

    /*
     * Periodic is called to ensure that the pid controller and feed forward
     * always have the most recent position in memory
     */
    @Override
    public void periodic() {
        super.periodic();

        pidControllerOutput = pidController.calculate(encoder.getPosition());

        feedForwardOutput = feedForward.calculate(pidController.getSetpoint().velocity);
    }

    /**
     * Sets the pid controller's goal
     * 
     * @param goal The desired goal
     */
    public void setGoal(double goal) {
        double clampedGoal = MathUtil.clamp(
            goal,
            0,
            kMaxExtension
        );

        pidController.setGoal(clampedGoal);
        return;
    }

    /**
     * Sets the pid controller's goal
     * 
     * @param goal The desired goal
     */
    public void setGoal(DoubleSupplier goal) {
        double clampedGoal = MathUtil.clamp(
            goal.getAsDouble(), 
            0, 
            kMaxExtension
        );

        pidController.setGoal(clampedGoal);
        return;
    }

    /**
     * Makes the elevator go to the current goal
     * 
     * @return Command that applies power to the motor based off of the pid controller's calculations
     */
    public Command goToPosition() {
        return run(() -> {
            motor1.setVoltage(
                pidControllerOutput + feedForwardOutput
            );
        });
    }
    /**
     * Makes the elevator go to the @goal param
     * 
     * @param goal The desired goal
     * @return Command that applies power to the motor based off of the pid controller's calculations
     */
    public Command goToPosition(double goal) {
        setGoal(goal);

        return run(() -> {
            motor1.setVoltage(
                pidControllerOutput + feedForwardOutput
            );
        });
    }

    /**
     * Runs the elevator to a set position slightly above home, then applies a constant voltage until velocity is equal to zero.
     * This then rezeros the encoder
     * 
     * @return Command that achieves a proper rezeroing position
     */
    public Command goToHome() {
        return run(() -> {
            goToPosition(kHomingLocation);
        }).until(pidController::atGoal).andThen(
            run(() -> {
                motor1.setVoltage(kHomingVoltage);
            }).withTimeout(kHomingDelay).andThen(run(() -> {
                motor1.setVoltage(kHomingVoltage);
            })).until(() -> encoder.getVelocity() <= kHomingVelocityLimit)
        ).finallyDo((interrupted) -> {
            if (!interrupted) {
                encoder.setPosition(0);
            }
        });
    }
}