import time
import krpc
import math

# ================= 配置参数 (V5 - 针对稳定性与精准度重置) =================
TARGET_ALTITUDE = 50.0   

# 垂直控制 (双环)
ALT_OUTER_KP = 0.3      # 适度增大外环增益，平衡响应速度与稳定性
ALT_INNER_KP = 0.05     # 适度增大速度环增益
ALT_INNER_KI = 0.03     # 增大积分项，消除稳态误差
ALT_INNER_KD = 0.25     # 增大阻尼，抑制过冲

# 水平控制 (双环 - 极端提前减速 + 暴力制动版)
POS_KP = 0.04          # 目标速 = 距离 * 0.04 (375m处就开始限制速度，实现极早制动)
MAX_H_VEL = 15.0       

VEL_KP = 0.12          # 增强力度，让姿态更果断地跟上速度变化
VEL_KI = 0.01          
VEL_KD = 0.80          # 极强阻尼，防止因大角度刹车导致的过冲和摆动
MAX_TILT = 0.50        # 允许最大倾角约 28 度！提供 SpaceX 级别的暴力水平制动力

DT = 0.1               # 采样周期
ALPHA = 0.3            # 节流阀滤波 (越小越平滑)
# =========================================================================

conn = krpc.connect(name='Hover Guidance V5')
if not conn.space_center:
    print("No active vessel found.")
    exit()
vessel = conn.space_center.active_vessel

class PID:
    def __init__(self, kp, ki, kd, limit=1.0):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.limit = limit
        self.integral = 0
        self.prev_error = 0

    def update(self, error, dt):
        self.integral = max(-self.limit, min(self.limit, self.integral + error * dt))
        derivative = (error - self.prev_error) / dt
        self.prev_error = error
        return (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative)

# 初始化所有控制器
alt_pid = PID(ALT_INNER_KP, ALT_INNER_KI, ALT_INNER_KD, limit=1.0)
vel_n_pid = PID(VEL_KP, VEL_KI, VEL_KD, limit=0.1)
vel_e_pid = PID(VEL_KP, VEL_KI, VEL_KD, limit=0.1)

# 初始化位置环积分
pos_n_integral = 0.0
pos_e_integral = 0.0

# # 给定目标点 (KSC 着陆台)
# target_lat = -0.097179090928
# target_lon = -74.5576787510

# VBA房顶
target_lat = -0.09684801697319373
target_lon = -74.61749199803717


body_radius = vessel.orbit.body.equatorial_radius
deg_to_m = (2 * math.pi * body_radius) / 360.0

# 准备状态
vessel.auto_pilot.engage()
vessel.auto_pilot.reference_frame = vessel.surface_reference_frame
last_throttle = 0.0

print(f"制导系统 V5 已启动，目标高度: {TARGET_ALTITUDE}m")

while True:
    # 1. 采集速度数据 (使用 surface_velocity_reference_frame 获取地表相对速度)
    v_surf = vessel.velocity(vessel.surface_velocity_reference_frame)
    v_up = v_surf[0]
    v_north = v_surf[1]
    v_east = v_surf[2]
    
    flight = vessel.flight(vessel.surface_reference_frame)
    alt = flight.surface_altitude
    lat = flight.latitude
    lon = flight.longitude

    # 2. 水平层逻辑 (制导)
    # 计算位移误差（米）
    err_n = (target_lat - lat) * deg_to_m
    err_e = (target_lon - lon) * deg_to_m * math.cos(math.radians(lat))
    
    # 调试信息打印 (提高精度到小数点后 6 位，显示微小速度变化)
    dist = math.sqrt(err_n**2 + err_e**2)
    
    # 【核心重构】：线性减速模型。
    # 我们不再使用复杂的五段式，而是使用一个平滑的比例：
    # 目标速度 = 距离 * 0.05。 
    # 比如在 100m 处，目标速度就是 5m/s。
    # 在 20m 处，目标速度就是 1m/s。
    # 这样火箭从 300m 外就会开始感觉到“目标速度在变小”，从而实现极早的反向机动。
    target_vn = max(-MAX_H_VEL, min(MAX_H_VEL, err_n * POS_KP))
    target_ve = max(-MAX_H_VEL, min(MAX_H_VEL, err_e * POS_KP))

    print(f"距离: {dist:.1f}m | 目标速: {math.sqrt(target_vn**2+target_ve**2):.1f} | N速: {v_north:.3f}")
    
    # 内环: 目标速 - 实速 -> 倾角
    tilt_n = vel_n_pid.update(target_vn - v_north, DT)
    tilt_e = vel_e_pid.update(target_ve - v_east, DT)
    
    # 限制并设置姿态
    tilt_n = max(-MAX_TILT, min(MAX_TILT, tilt_n))
    tilt_e = max(-MAX_TILT, min(MAX_TILT, tilt_e))
    vessel.auto_pilot.target_direction = (1.0, tilt_n, tilt_e)

    # 3. 垂直层逻辑
    # 外环: 高度误差 -> 目标垂直速 (P)
    target_vs = (TARGET_ALTITUDE - alt) * ALT_OUTER_KP
    target_vs = max(-10.0, min(10.0, target_vs))
    
    # 内环: 速度误差 -> 节流阀 (PID)
    raw_throttle = alt_pid.update(target_vs - v_up, DT)
    
    # 加权补偿 (抵消因垂直向量偏置导致的升力损失)
    cos_tilt = math.cos(math.sqrt(tilt_n**2 + tilt_e**2))
    throttle_corr = raw_throttle / max(0.8, cos_tilt)

    # 4. 滤波输出
    throttle = (ALPHA * throttle_corr) + ((1.0 - ALPHA) * last_throttle)
    throttle = max(0.0, min(1.0, throttle))
    vessel.control.throttle = throttle
    last_throttle = throttle

    # 5. 信息反馈
    dist = math.sqrt(err_n**2 + err_e**2)
    print(f"距离: {dist:.2f}m | 高度: {alt:.1f}m | N_速: {v_north:.2f}m/s | 节流阀: {throttle:.2f}")
    
    time.sleep(DT)