package frc.robot.controls;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;

import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.RobotStatusManager;
import frc.robot.Constants.DriveMode;
import frc.robot.Constants.RobotStatus;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;

import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; 

public class DriveControls {
    public boolean isRedAlliance;

    private final CommandSwerveDrivetrain drivetrain;
    private final CommandXboxController driver;

    private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
    private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);

    // 默认速度比例（平时开车）
    private double normalDriveScale = 0.3;
    private double normalTurnScale  = 0.7;

    // 加速模式比例（按住 RB）
    private double boostDriveScale = 1.0;
    private double boostTurnScale  = 1.0;

    private boolean boostEnabled = false;

    private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0);   // m/s^2 等效（调）
    private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0);
    private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效（调）

    // ====== 类成员变量里新增：记录上一次输出 ======
    private double lastVx = 0.0;
    private double lastVy = 0.0;
    private double lastOmega = 0.0;

    private NetworkTable autoControlNetworkTable;
    
    private final RobotStatusManager robotStatusManager;
    public void setBoostEnabled(boolean enabled) {
        boostEnabled = enabled;
    }

    // 新增：当前是否使用场地坐标系（默认 true）
    private boolean fieldCentricEnabled = true;

    private final SwerveRequest.FieldCentric fieldCentric =
        new SwerveRequest.FieldCentric()
            .withDeadband(maxSpeed * 0.05)
            .withRotationalDeadband(maxAngularRate * 0.05)
            .withDriveRequestType(DriveRequestType.Velocity);

    private final SwerveRequest.FieldCentric fieldCentricAuto =
    new SwerveRequest.FieldCentric()
        .withDeadband(maxSpeed * 0.05)
        .withRotationalDeadband(maxAngularRate * 0.05)
        .withDriveRequestType(DriveRequestType.Velocity)
        .withForwardPerspective(SwerveRequest.ForwardPerspectiveValue.BlueAlliance);

    private final SwerveRequest.RobotCentric robotCentric =
    new SwerveRequest.RobotCentric()
        .withDeadband(maxSpeed * 0.05)
        .withRotationalDeadband(maxAngularRate * 0.05)
        .withDriveRequestType(DriveRequestType.Velocity);
    
    public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) {
        this.drivetrain = drivetrain;
        this.driver = driver;
        this.robotStatusManager = rsm;
        autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
    }
    
    public void setFieldCentricEnabled(boolean enabled) {
        fieldCentricEnabled = enabled;
    }
    
    public boolean isFieldCentricEnabled() {
        return fieldCentricEnabled;
    }

    /** 底盘的默认驾驶命令（teleop 期间持续执行） */
    public Command defaultDriveCommand() {
        return drivetrain.applyRequest(() -> {
        SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
        double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
        double turnScale  = boostEnabled ? boostTurnScale  : normalTurnScale;
        double vx = -driver.getLeftY() * maxSpeed * driveScale;
        double vy = -driver.getLeftX() * maxSpeed * driveScale;
        double vomega = -driver.getRightX() * maxAngularRate * turnScale;
        // ====== execute 内替换 limiter：只限加速、不限减速 ======
        double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
        double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
        double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter);

        lastVx = limitedVx;
        lastVy = limitedVy;
        lastOmega = limitedOmega;

        vx = limitedVx;
        vy = limitedVy;
        vomega = limitedOmega;

        double[] distanceAndRotation = calculateDistanceAndRotationToHub();
        // 将距离值写入 NetworkTable
        autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
        autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);

        switch (robotStatusManager.getStatus()) {
            case Stopped:
                vx = 0;
                vy = 0;
                vomega = 0;
                break;
            case PassingBall:
                vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
                break;
            case AutoAimming:
                vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
                break;
            case AllTelop:
                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                break;
            case Climbing:
                break;
            case CrossingBump:
                double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
                if (Math.abs(differenceBump) < 0.1) {
                    vy = 0;
                } else {
                    double raw = differenceBump * Constants.AutoPositioning.autoPositioningkP;
                    vy = Math.copySign(
                        Math.max(Math.abs(raw), 0.4),
                        raw
                    );
                }
                autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);

                Rotation2d BumpcurrentAngle = drivetrain.getState().Pose.getRotation();
                double BumpcurrentRad = BumpcurrentAngle.getRadians();
                double targetA = Math.toRadians(45);
                double targetB = Math.toRadians(-45);
                // 计算误差
                double errorA = targetA - BumpcurrentRad;
                double errorB = targetB - BumpcurrentRad;
                // 角度归一化到 [-pi, pi]
                errorA = Math.atan2(Math.sin(errorA), Math.cos(errorA));
                errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
                // 选误差绝对值更小的那个
                double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
                vomega = calculatevomegaFromRotationAngle(chosenError);
                autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(vomega);
                //vomega = calculateRotationSpeedForBump();
                break;
            case CrossingTrench:
                double differenceTrench = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.trenchY[0], Constants.AutoPositioning.trenchY[1]);
                if (Math.abs(differenceTrench) < 0.1) {
                    vy = 0;
                } else {
                    double raw = differenceTrench * Constants.AutoPositioning.autoPositioningkP;
                    vy = Math.copySign(
                        Math.max(Math.abs(raw), 0.4),
                        raw
                    );
                }
                autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
                Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
                double currentRad = currentAngle.getRadians();
                double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
                double TrenchAngleerror = targetRad - currentRad;
                vomega = calculatevomegaFromRotationAngle(TrenchAngleerror);
                autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(vomega);
                //vomega = calculateRotationSpeedForTrench();
                break;
        }        

        autoControlNetworkTable.getEntry("ActualVX").setDouble(vx);
        autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
        autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
        
        boolean isAutoLike =
            robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
            robotStatusManager.getStatus() == RobotStatus.CrossingTrench;

        if (fieldCentricEnabled || isAutoLike) {

    // 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
        var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
        return chosen
                .withVelocityX(vx)
                .withVelocityY(vy)
                .withRotationalRate(vomega);
        } else {
            return robotCentric
                .withVelocityX(vx)
                .withVelocityY(vy)
                .withRotationalRate(vomega);
        }
        }, () -> fieldCentricEnabled || 
            robotStatusManager.getStatus() == RobotStatus.CrossingBump || 
            robotStatusManager.getStatus() == RobotStatus.CrossingTrench
             ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
    }

    /**
     * 
         * 输出移动到距离两个目标之间较近的那个点的位移
         * @return 位移 有正负
         */
    public double calculateDifferenceToTwoTarget(double current, double target1, double target2){
          double x1 = target1 - current;
          double x2 = target2 - current;
          return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
    }
    public double[] calculateDistanceAndRotationToHub() {
        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
        double targetX = Constants.Field.RedHubPositionX;  // 目标 X 坐标
        double targetY = Constants.Field.RedHubPositionY;  // 目标 Y 坐标

        // 计算目标角度（相对于场地坐标系）
        double deltaX = targetX - currentPose.getX();
        double deltaY = targetY - currentPose.getY();
        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度

        // 当前机器人角度
        double currentAngle = currentPose.getRotation().getDegrees();
        double angleDifference = targetAngle - currentAngle;
        // Normalize angle difference to the range [-180, 180]
        if (angleDifference > 180) {
            angleDifference -= 360;
        } else if (angleDifference < -180) {
            angleDifference += 360;
        }

        // 计算距离
        double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
                // 计算距离和旋转角度
        double[] distanceAndRotation = new double[]{distance, angleDifference};

        // 返回距离和角度差
        return distanceAndRotation;
    }
    public double[] calculateDistanceAndRotationToPassBall() {
        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
        double targetX = Constants.Field.PassingBallPosX;  // 目标 X 坐标
        double targetY1 = Constants.Field.PassingBallPosY1;  // 目标 Y 坐标
        double targetY2 = Constants.Field.PassingBallPosY2;  // 目标 Y 坐标
       
        // 计算目标角度（相对于场地坐标系）
        double deltaX = targetX - currentPose.getX();
        double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度

        // 当前机器人角度
        double currentAngle = currentPose.getRotation().getDegrees();
        double angleDifference = targetAngle - currentAngle;
        // Normalize angle difference to the range [-180, 180]
        if (angleDifference > 180) {
            angleDifference -= 360;
        } else if (angleDifference < -180) {
            angleDifference += 360;
        }

        // 计算距离
        double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
                // 计算距离和旋转角度
        double[] distanceAndRotation = new double[]{distance, angleDifference};

        // 返回距离和角度差
        return distanceAndRotation;
    }

    public double calculateRotationSpeedFromRotationAngle(double angleDifference) {
        double calDifference = angleDifference;

        // Optional: Apply teleop offset if enabled
        if (Constants.AutoPositioning.teleopOffsetEnabled) {
            double teleopOffset = -driver.getRightX();
            calDifference += teleopOffset * Constants.AutoPositioning.teleopOffsetkP;
        }

        // If angle difference is within threshold, don't rotate
        if (Math.abs(calDifference) > 1.0) {
            return calDifference  * Constants.AutoPositioning.autoRotationkP;
        } else {
            return 0;
        }
    }
    
    private double calculatevomegaFromRotationAngle(double error) {
        double vomega;
        if (Math.abs(error) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
                vomega = 0.0;
            } else {
                vomega = error * Constants.AutoPositioning.TurningkP;
            }
            // 限制最大角速度
            vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
        return vomega;
    }

        /**
     * 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
     * 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击
     */
    private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) {
        // 如果正在减速（幅值变小），直接放行到目标值
        if (Math.abs(target) < Math.abs(last)) {
        limiter.reset(target);  // 同步 limiter 内部状态，防止下一次突变
        return target;
        }
        // 否则是加速，使用 limiter
        return limiter.calculate(target);
    }

        /** 紧急停止：清零限速器，让速度输出立刻归零 */
    public void emergencyStop() {
        vxLimiter.reset(0.0);
        vyLimiter.reset(0.0);
        omegaLimiter.reset(0.0);

        lastVx = 0.0;
        lastVy = 0.0;
        lastOmega = 0.0;
    }

    public Command idleCommand() {
        final var idle = new SwerveRequest.Idle();
        return drivetrain.applyRequest(() -> idle).ignoringDisable(true);
    }
    
    public Command brakeWhileHeld() {
        final var brake = new SwerveRequest.SwerveDriveBrake();
        return drivetrain.applyRequest(() -> brake);
    }
    

    public SwerveRequest.SwerveDriveBrake brakeRequest() {
        return new SwerveRequest.SwerveDriveBrake();
    }

    public SwerveRequest.Idle idleRequest() {
        return new SwerveRequest.Idle();
    }

    public double getMaxSpeed() {
        return maxSpeed;
    }

    public double getMaxAngularRate() {
        return maxAngularRate;
    }
    public void setTeamColors() {
        Optional<Alliance> ally = DriverStation.getAlliance(); // 获取队伍颜色
        if (ally.isPresent()) {
            // 根据队伍颜色设置布尔值
            if (ally.get() == Alliance.Red) {
                isRedAlliance = true; // 红队
            } else if (ally.get() == Alliance.Blue) {
                isRedAlliance = false; // 蓝队
            }
        } else {
            isRedAlliance = true;
        }
        if (isRedAlliance) {
            System.out.println("Robot is on the Red Alliance.");
        } else {
            System.out.println("Robot is on the Blue Alliance.");
        }
    }
    public Command getAllianceColorCommand() {
        return Commands.runOnce(this::setTeamColors);
}

}