package com.algo.model;

/**
 * CTU物理参数配置类
 * 包含CTU运动的各种物理参数，用于路径规划和碰撞检测
 */
public class CTUPhysicalConfig {

    /**
     * 最大移动速度 (米/秒)
     */
    private double maxSpeed;

    /**
     * 正常移动速度 (米/秒)
     */
    private double normalSpeed;

    /**
     * 最大加速度 (米/秒²)
     */
    private double maxAcceleration;

    /**
     * 最大减速度 (米/秒²)
     */
    private double maxDeceleration;

    /**
     * 转向时间 (秒) - 90度转向所需时间
     */
    private double turnTime90;

    /**
     * 转向时间 (秒) - 180度转向所需时间
     */
    private double turnTime180;

    /**
     * CTU之间的最小安全距离 (米)
     */
    private double minSafetyDistance;

    /**
     * CTU之间的最小跟随距离 (米)
     */
    private double minFollowingDistance;

    /**
     * CTU的物理长度 (米)
     */
    private double ctuLength;

    /**
     * CTU的物理宽度 (米)
     */
    private double ctuWidth;

    /**
     * 启动时间 (秒) - 从静止到正常速度所需时间
     */
    private double startupTime;

    /**
     * 停止时间 (秒) - 从正常速度到静止所需时间
     */
    private double stopTime;

    /**
     * 路径点之间的标准距离 (米)
     */
    private double standardPointDistance;

    // 默认构造函数 - 使用推荐的默认值
    public CTUPhysicalConfig() {
        this.maxSpeed = 2.0;              // 2米/秒
        this.normalSpeed = 1.5;           // 1.5米/秒
        this.maxAcceleration = 1.0;       // 1米/秒²
        this.maxDeceleration = 1.5;       // 1.5米/秒²
        this.turnTime90 = 2.0;            // 90度转向需要2秒
        this.turnTime180 = 4.0;           // 180度转向需要4秒
        this.minSafetyDistance = 3.0;     // 最小安全距离3米
        this.minFollowingDistance = 2.0;  // 最小跟随距离2米
        this.ctuLength = 1.5;             // CTU长度1.5米
        this.ctuWidth = 1.0;              // CTU宽度1米
        this.startupTime = 1.0;           // 启动时间1秒
        this.stopTime = 1.5;              // 停止时间1.5秒
        this.standardPointDistance = 1.0; // 标准路径点距离1米
    }

    // 完整参数的构造函数
    public CTUPhysicalConfig(double maxSpeed, double normalSpeed, double maxAcceleration,
                             double maxDeceleration, double turnTime90, double turnTime180,
                             double minSafetyDistance, double minFollowingDistance,
                             double ctuLength, double ctuWidth, double startupTime,
                             double stopTime, double standardPointDistance) {
        this.maxSpeed = maxSpeed;
        this.normalSpeed = normalSpeed;
        this.maxAcceleration = maxAcceleration;
        this.maxDeceleration = maxDeceleration;
        this.turnTime90 = turnTime90;
        this.turnTime180 = turnTime180;
        this.minSafetyDistance = minSafetyDistance;
        this.minFollowingDistance = minFollowingDistance;
        this.ctuLength = ctuLength;
        this.ctuWidth = ctuWidth;
        this.startupTime = startupTime;
        this.stopTime = stopTime;
        this.standardPointDistance = standardPointDistance;
    }

    // Getter和Setter方法
    public double getMaxSpeed() {
        return maxSpeed;
    }

    public void setMaxSpeed(double maxSpeed) {
        this.maxSpeed = maxSpeed;
    }

    public double getNormalSpeed() {
        return normalSpeed;
    }

    public void setNormalSpeed(double normalSpeed) {
        this.normalSpeed = normalSpeed;
    }

    public double getMaxAcceleration() {
        return maxAcceleration;
    }

    public void setMaxAcceleration(double maxAcceleration) {
        this.maxAcceleration = maxAcceleration;
    }

    public double getMaxDeceleration() {
        return maxDeceleration;
    }

    public void setMaxDeceleration(double maxDeceleration) {
        this.maxDeceleration = maxDeceleration;
    }

    public double getTurnTime90() {
        return turnTime90;
    }

    public void setTurnTime90(double turnTime90) {
        this.turnTime90 = turnTime90;
    }

    public double getTurnTime180() {
        return turnTime180;
    }

    public void setTurnTime180(double turnTime180) {
        this.turnTime180 = turnTime180;
    }

    public double getMinSafetyDistance() {
        return minSafetyDistance;
    }

    public void setMinSafetyDistance(double minSafetyDistance) {
        this.minSafetyDistance = minSafetyDistance;
    }

    public double getMinFollowingDistance() {
        return minFollowingDistance;
    }

    public void setMinFollowingDistance(double minFollowingDistance) {
        this.minFollowingDistance = minFollowingDistance;
    }

    public double getCtuLength() {
        return ctuLength;
    }

    public void setCtuLength(double ctuLength) {
        this.ctuLength = ctuLength;
    }

    public double getCtuWidth() {
        return ctuWidth;
    }

    public void setCtuWidth(double ctuWidth) {
        this.ctuWidth = ctuWidth;
    }

    public double getStartupTime() {
        return startupTime;
    }

    public void setStartupTime(double startupTime) {
        this.startupTime = startupTime;
    }

    public double getStopTime() {
        return stopTime;
    }

    public void setStopTime(double stopTime) {
        this.stopTime = stopTime;
    }

    public double getStandardPointDistance() {
        return standardPointDistance;
    }

    public void setStandardPointDistance(double standardPointDistance) {
        this.standardPointDistance = standardPointDistance;
    }

    /**
     * 计算从当前速度加速到目标速度所需的时间
     *
     * @param currentSpeed 当前速度
     * @param targetSpeed  目标速度
     * @return 所需时间（秒）
     */
    public double getAccelerationTime(double currentSpeed, double targetSpeed) {
        if (targetSpeed > currentSpeed) {
            return (targetSpeed - currentSpeed) / maxAcceleration;
        } else {
            return (currentSpeed - targetSpeed) / maxDeceleration;
        }
    }

    /**
     * 计算从当前速度加速/减速到目标速度所需的距离
     *
     * @param currentSpeed 当前速度
     * @param targetSpeed  目标速度
     * @return 所需距离（米）
     */
    public double getAccelerationDistance(double currentSpeed, double targetSpeed) {
        double time = getAccelerationTime(currentSpeed, targetSpeed);
        return (currentSpeed + targetSpeed) * time / 2.0;
    }

    /**
     * 根据路径点距离计算移动时间
     *
     * @param distance     距离（米）
     * @param currentSpeed 当前速度
     * @return 移动时间（秒）
     */
    public double getTravelTime(double distance, double currentSpeed) {
        if (currentSpeed <= 0) {
            currentSpeed = normalSpeed;
        }
        return distance / currentSpeed;
    }

    /**
     * 根据方向变化计算转向时间
     *
     * @param fromDirection 起始方向角度
     * @param toDirection   目标方向角度
     * @return 转向时间（秒）
     */
    public double getTurnTime(String fromDirection, String toDirection) {
        try {
            double from = Double.parseDouble(fromDirection);
            double to = Double.parseDouble(toDirection);

            // 计算角度差
            double angleDiff = Math.abs(to - from);
            if (angleDiff > 180) {
                angleDiff = 360 - angleDiff;
            }

            // 根据角度差计算转向时间
            if (angleDiff <= 5) { // 基本无转向
                return 0.0;
            } else if (angleDiff <= 90) {
                return turnTime90 * (angleDiff / 90.0);
            } else if (angleDiff <= 180) {
                return turnTime90 + (turnTime180 - turnTime90) * ((angleDiff - 90) / 90.0);
            } else {
                return turnTime180;
            }
        } catch (NumberFormatException e) {
            // 如果方向不是数字，返回默认转向时间
            return turnTime90;
        }
    }

    /**
     * 检查两个CTU之间的距离是否满足安全要求
     *
     * @param distance    距离（米）
     * @param isFollowing 是否为跟随情况
     * @return true如果距离安全
     */
    public boolean isSafeDistance(double distance, boolean isFollowing) {
        double minDistance = isFollowing ? minFollowingDistance : minSafetyDistance;
        return distance >= minDistance;
    }

    @Override
    public String toString() {
        return "CTUPhysicalConfig{" +
                "maxSpeed=" + maxSpeed +
                ", normalSpeed=" + normalSpeed +
                ", maxAcceleration=" + maxAcceleration +
                ", maxDeceleration=" + maxDeceleration +
                ", turnTime90=" + turnTime90 +
                ", turnTime180=" + turnTime180 +
                ", minSafetyDistance=" + minSafetyDistance +
                ", minFollowingDistance=" + minFollowingDistance +
                ", ctuLength=" + ctuLength +
                ", ctuWidth=" + ctuWidth +
                ", startupTime=" + startupTime +
                ", stopTime=" + stopTime +
                ", standardPointDistance=" + standardPointDistance +
                '}';
    }
}