"""夹爪定时开合 + 双触觉反馈控制配置。

这个文件就是当前默认配置。可以直接改数值，`#` 后面是 Python 注释。
运行入口：
    python -m gripper_control.main
"""


# 左右触觉传感器视频源；纯数字字符串表示摄像头编号，也可以写视频文件路径。
VIDEO1 = "0"
VIDEO2 = "1"

# 左右触觉传感器方向配置。
ROTATION1 = "./config/rotation_config_0.json"
ROTATION2 = "./config/rotation_config_1.json"

# 左右触觉传感器 SDK 配置。
SENSOR_CONFIG1 = "./config/ddjx01.json"
SENSOR_CONFIG2 = "./config/ddjx01.json"

# 触觉处理帧率和运动检测阈值。
SENSOR_FPS = 30.0
MOTION_THRESHOLD = 0.0
SENSOR_CPU = False  # True 表示禁用 CUDA。


# 夹爪串口配置。
PORT = "COM5"
SLAVE_ID = 1
BAUDRATE = 115200
SERIAL_TIMEOUT = 0.2


# 夹爪开合位置和定时循环。
OPEN_POS = 0
CLOSE_POS = 9000
OPEN_SECONDS = 3.0       # 未夹住物体时，张开阶段持续时间。
CLOSE_SECONDS = 5.0      # 未夹住物体时，闭合尝试持续时间。
CYCLES = 0               # 0 表示一直循环，直到夹住物体或手动停止。
OPEN_AT_END = False      # 程序退出时是否发送一次张开命令。


# 夹爪运动参数。
SPEED = 15
ACCEL = 60
DECEL = 60


# 夹爪力度参数，单位是夹爪 SDK 的 force_pct 百分比。
INITIAL_FORCE = 15       # 定时闭合时用小力，避免空夹/误触发时用大力。
GRIP_START_FORCE = 15    # 检测到物体后立刻切回这个低力，再慢慢加力。
FORCE_MIN = 10
FORCE_MAX = 30
CONTROL_HZ = 10.0


# 法向力阈值，单位 N。程序会先做标定映射，再做判断。
BOTH_CONTACT_FORCE = 0.05          # 左右两边都超过它时 both=1。
OBJECT_FORCE = 0.08                # 检测到物体的法向力阈值。
OBJECT_REQUIRES_BOTH_CONTACT = True  # True 可避免空夹时单侧噪声误触发。
EMPTY_RELEASE_SECONDS = 1.0        # 夹持后接触消失这么久，判定为空夹并恢复定时开合。


# 未检测到物体时，重复发送闭合命令的间隔；0 表示不重复发送。
CLOSE_COMMAND_INTERVAL = 0.5

# 读取当前夹爪位置失败时的备用保持位置；None 表示不使用备用位置。
HOLD_FALLBACK_POS = None


# 检测到物体后慢慢加力。
FORCE_RAMP_STEP = 1
FORCE_RAMP_INTERVAL = 0.3


# 切向力判断，单位 N。当前切向力标定暂时沿用 N_.jpg，后续有切向标定后替换。
SHEAR_HOLD_FORCE = 0.05       # 切向力超过它后，停止继续加力，开始等稳定。
SHEAR_STABLE_DELTA = 0.02     # 相邻控制周期变化小于等于它，认为这一拍稳定。
SHEAR_STABLE_SECONDS = 0.5    # 连续稳定这么久后进入 hold_check。
HUMAN_HOLD_SECONDS = 1.0      # 进入 hold_check 后先稳定保持这么久，再判断人取物。
RELEASE_SHEAR_CHANGE = 0.5    # hold_check 后切向力变化超过它，认为人在取物并松开。


# 高法向力保护，单位 N。
EMERGENCY_TOUCH_FORCE = 2.5


# 滤波和启动基线。
FILTER_ALPHA = 0.25
SHEAR_FILTER_ALPHA = 0.35
BASELINE_SECONDS = 1.0


# 法向力标定：FNORMAL 原始值 -> N。
NORMAL_FORCE_CALIBRATION = {
    "enabled": True,
    "method": "piecewise_linear",
    "extrapolate": True,
    "clamp_output_min": 0.0,
    "points": [
        {"fnormal": 1000.0, "force_n": 0.0},       # 0 N
        {"fnormal": 10000.0, "force_n": 0.377},    # 0.377 N
        {"fnormal": 30000.0, "force_n": 1.377},    # 1.377 N
        {"fnormal": 62000.0, "force_n": 2.377},    # 2.377 N
    ],
}


# 切向力标定：sqrt(FSHEARX^2 + FSHEARY^2) 原始幅值 -> N。
# 注意：这里目前临时沿用 N_.jpg 的法向力标定点。
# 后续如果你做了切向力标定，只需要替换 points。
SHEAR_FORCE_CALIBRATION = {
    "enabled": True,
    "method": "piecewise_linear",
    "extrapolate": True,
    "clamp_output_min": 0.0,
    "points": [
        {"raw": 1000.0, "force_n": 0.0},
        {"raw": 10000.0, "force_n": 0.377},
        {"raw": 30000.0, "force_n": 1.377},
        {"raw": 62000.0, "force_n": 2.377},
    ],
}


# 夹爪 SDK 细节开关。
TRIGGER_FORCE_UPDATE = False  # 如果 set_target_force 后力度不生效，可改 True。
ENABLE_HUMAN_RELEASE = True   # True 表示人拿物体时，切向力变化会触发松开。
ENABLE_EMERGENCY_OPEN = False # True 表示超过 EMERGENCY_TOUCH_FORCE 时自动张开。
DRY_RUN = False               # True 只打印命令，不打开串口。


# 运行代码最终读取这个字典。
CONFIG = {
    "video1": VIDEO1,
    "video2": VIDEO2,
    "rotation1": ROTATION1,
    "rotation2": ROTATION2,
    "sensor_config1": SENSOR_CONFIG1,
    "sensor_config2": SENSOR_CONFIG2,
    "sensor_fps": SENSOR_FPS,
    "motion_threshold": MOTION_THRESHOLD,
    "sensor_cpu": SENSOR_CPU,

    "port": PORT,
    "slave_id": SLAVE_ID,
    "baudrate": BAUDRATE,
    "serial_timeout": SERIAL_TIMEOUT,

    "open_pos": OPEN_POS,
    "close_pos": CLOSE_POS,
    "open_seconds": OPEN_SECONDS,
    "close_seconds": CLOSE_SECONDS,
    "cycles": CYCLES,
    "open_at_end": OPEN_AT_END,

    "speed": SPEED,
    "accel": ACCEL,
    "decel": DECEL,

    "initial_force": INITIAL_FORCE,
    "grip_start_force": GRIP_START_FORCE,
    "force_min": FORCE_MIN,
    "force_max": FORCE_MAX,
    "control_hz": CONTROL_HZ,

    "both_contact_force": BOTH_CONTACT_FORCE,
    "object_force": OBJECT_FORCE,
    "object_requires_both_contact": OBJECT_REQUIRES_BOTH_CONTACT,
    "empty_release_seconds": EMPTY_RELEASE_SECONDS,
    "close_command_interval": CLOSE_COMMAND_INTERVAL,
    "hold_fallback_pos": HOLD_FALLBACK_POS,

    "force_ramp_step": FORCE_RAMP_STEP,
    "force_ramp_interval": FORCE_RAMP_INTERVAL,
    "shear_hold_force": SHEAR_HOLD_FORCE,
    "shear_stable_delta": SHEAR_STABLE_DELTA,
    "shear_stable_seconds": SHEAR_STABLE_SECONDS,
    "human_hold_seconds": HUMAN_HOLD_SECONDS,
    "release_shear_change": RELEASE_SHEAR_CHANGE,

    "emergency_touch_force": EMERGENCY_TOUCH_FORCE,
    "filter_alpha": FILTER_ALPHA,
    "shear_filter_alpha": SHEAR_FILTER_ALPHA,
    "baseline_seconds": BASELINE_SECONDS,

    "normal_force_calibration": NORMAL_FORCE_CALIBRATION,
    "shear_force_calibration": SHEAR_FORCE_CALIBRATION,

    "trigger_force_update": TRIGGER_FORCE_UPDATE,
    "enable_human_release": ENABLE_HUMAN_RELEASE,
    "enable_emergency_open": ENABLE_EMERGENCY_OPEN,
    "dry_run": DRY_RUN,
}