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 = 1.0;              // 1000mm/s = 1.0米/秒
        this.normalSpeed = 1.0;           // 正常运行速度100%最大速度
        this.maxAcceleration = 0.4;       // 400 mm/s^2 = 0.4米/秒²
        this.maxDeceleration = 0.4;       // 相同的加减速度
        this.turnTime90 = 4.0;            // 每90度转向需要4秒
        this.turnTime180 = 8.0;           // 180度转向需要8秒
        this.minSafetyDistance = 3.0;     // 最小安全距离3米
        this.minFollowingDistance = 3.0;  // 最小跟随距离3米
        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;
    }

    /**
     * 计算精确的移动时间，考虑加速度、减速度和实际距离
     *
     * @param distance       实际移动距离（米）
     * @param startSpeed     起始速度（米/秒）
     * @param endSpeed       结束速度（米/秒）
     * @param targetSpeed    目标巡航速度（米/秒）
     * @return 移动时间（秒）
     */
    public double calculatePreciseMovementTime(double distance, double startSpeed, double endSpeed, double targetSpeed) {
        if (distance <= 0) return 0.0;
        
        // 如果距离很短，直接计算平均速度
        if (distance < 0.5) { // 小于0.5米
            double avgSpeed = Math.max(0.1, (startSpeed + endSpeed) / 2.0);
            return distance / avgSpeed;
        }
        
        double totalTime = 0.0;
        double currentSpeed = startSpeed;
        double remainingDistance = distance;
        
        // 阶段1: 加速到目标速度
        if (currentSpeed < targetSpeed) {
            double accelerationTime = (targetSpeed - currentSpeed) / maxAcceleration;
            double accelerationDistance = currentSpeed * accelerationTime + 0.5 * maxAcceleration * accelerationTime * accelerationTime;
            
            if (accelerationDistance < remainingDistance) {
                totalTime += accelerationTime;
                remainingDistance -= accelerationDistance;
                currentSpeed = targetSpeed;
            } else {
                // 距离不足以完全加速，直接计算
                return calculateTimeForShortDistance(distance, startSpeed, endSpeed);
            }
        }
        
        // 阶段2: 减速到目标结束速度
        double decelerationDistance = 0.0;
        double decelerationTime = 0.0;
        if (currentSpeed > endSpeed) {
            decelerationTime = (currentSpeed - endSpeed) / maxDeceleration;
            decelerationDistance = endSpeed * decelerationTime + 0.5 * maxDeceleration * decelerationTime * decelerationTime;
        }
        
        // 阶段3: 匀速巡航
        if (remainingDistance > decelerationDistance) {
            double cruisingDistance = remainingDistance - decelerationDistance;
            double cruisingTime = cruisingDistance / currentSpeed;
            totalTime += cruisingTime;
        }
        
        // 添加减速时间
        totalTime += decelerationTime;
        
        return totalTime;
    }
    
    /**
     * 计算短距离移动时间（无法完整加速减速的情况）
     */
    private double calculateTimeForShortDistance(double distance, double startSpeed, double endSpeed) {
        // 使用运动学公式: s = v0*t + 0.5*a*t^2
        // 其中 v_f = v0 + a*t，求解 t
        double avgAcceleration = (endSpeed > startSpeed) ? maxAcceleration : -maxDeceleration;
        
        // 如果加速度很小或为0，使用平均速度
        if (Math.abs(avgAcceleration) < 0.01) {
            return distance / Math.max(0.1, (startSpeed + endSpeed) / 2.0);
        }
        
        // 求解二次方程: 0.5*a*t^2 + v0*t - s = 0
        double a = 0.5 * avgAcceleration;
        double b = startSpeed;
        double c = -distance;
        
        double discriminant = b * b - 4 * a * c;
        if (discriminant < 0) {
            // 无解，使用平均速度
            return distance / Math.max(0.1, (startSpeed + endSpeed) / 2.0);
        }
        
        double t1 = (-b + Math.sqrt(discriminant)) / (2 * a);
        double t2 = (-b - Math.sqrt(discriminant)) / (2 * a);
        
        // 选择正的时间值
        double time = (t1 > 0) ? t1 : t2;
        return Math.max(0.1, time); // 确保时间为正
    }
    
    /**
     * 基于实际坐标计算两点间的精确距离（米）
     *
     * @param coord1 起点坐标 [x_mm, y_mm]
     * @param coord2 终点坐标 [x_mm, y_mm]
     * @return 距离（米）
     */
    public static double calculateRealDistance(double[] coord1, double[] coord2) {
        if (coord1 == null || coord2 == null || coord1.length < 2 || coord2.length < 2) {
            return 0.0;
        }
        
        // 坐标单位为毫米，转换为米
        double dx = (coord2[0] - coord1[0]) / 1000.0;
        double dy = (coord2[1] - coord1[1]) / 1000.0;
        
        return Math.sqrt(dx * dx + dy * dy);
    }
    
    /**
     * 计算方向改变角度
     *
     * @param coord1 起点坐标
     * @param coord2 中间点坐标  
     * @param coord3 终点坐标
     * @return 转向角度（度）
     */
    public static double calculateTurnAngle(double[] coord1, double[] coord2, double[] coord3) {
        if (coord1 == null || coord2 == null || coord3 == null) {
            return 0.0;
        }
        
        // 计算两个向量
        double[] vec1 = {coord2[0] - coord1[0], coord2[1] - coord1[1]};
        double[] vec2 = {coord3[0] - coord2[0], coord3[1] - coord2[1]};
        
        // 计算向量长度
        double len1 = Math.sqrt(vec1[0] * vec1[0] + vec1[1] * vec1[1]);
        double len2 = Math.sqrt(vec2[0] * vec2[0] + vec2[1] * vec2[1]);
        
        if (len1 < 1e-6 || len2 < 1e-6) {
            return 0.0;
        }
        
        // 计算夹角
        double dot = vec1[0] * vec2[0] + vec1[1] * vec2[1];
        double cosAngle = dot / (len1 * len2);
        
        // 限制cosAngle在[-1, 1]范围内
        cosAngle = Math.max(-1.0, Math.min(1.0, cosAngle));
        
        double angle = Math.acos(cosAngle);
        return Math.toDegrees(angle);
    }

    @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 +
                '}';
    }
}