"""夹爪02展示配置。

展示逻辑：
1. 程序启动后夹爪默认张开。
2. 等待任意一侧触觉传感器感受到力。
3. 一侧触发后，夹爪开始闭合；你把物体移动到中间。
4. 闭合时检测到物体后，像 01 一样低力保持当前位置并慢慢加力夹住。
5. 夹住后法向力或切向力变化超过阈值，直接张开到 OPEN_POS。
6. 张开后保持初始张开状态，等力清空并稳定后才重新等待单侧触发；不会自己定时开合。
"""


# 左右触觉传感器视频源。
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


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


# 夹爪位置。
OPEN_POS = 0
CLOSE_POS = 9000

# 夹爪运动参数。展示时可以速度稍快一点。
SPEED = 25
ACCEL = 80
DECEL = 80


# 夹爪力度百分比。
OPEN_FORCE = 15           # 张开时使用的力。
CLOSE_FORCE = 15          # 单侧触发后闭合时使用的小力。
GRIP_START_FORCE = 10     # 闭合检测到物体后，先切到这个低力保持。
HOLD_FORCE = 15           # 慢慢加力到这个值后进入 hold_check。
FORCE_MIN = 10
FORCE_MAX = 30


# 控制循环频率。展示时调高一点，单侧快速扫过不容易漏检。
CONTROL_HZ = 30.0


# 启动时采集空载基线的时间。
BASELINE_SECONDS = 1.0

# 滤波系数，越大响应越快，越小越平滑。
FILTER_ALPHA = 0.55
SHEAR_FILTER_ALPHA = 0.65


# 等待触发：任意一侧超过这些阈值，就开始闭合。
TRIGGER_NORMAL_FORCE = 0.04  # 单侧法向力触发阈值，单位 N。
TRIGGER_SHEAR_FORCE = 0.04   # 单侧切向力触发阈值，单位 N。

# 等待触发：如果已经记录了张开时的安静参考值，任意一侧变化超过这些阈值也会闭合。
# 这样现场轻扫一下也能触发，不完全依赖绝对力超过 TRIGGER_*_FORCE。
TRIGGER_NORMAL_CHANGE = 0.04  # 单侧法向力相对张开参考值的变化触发阈值，单位 N。
TRIGGER_SHEAR_CHANGE = 0.04   # 单侧切向力相对张开参考值的变化触发阈值，单位 N。


# 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。
GRIP_NORMAL_FORCE = 0.05     # 双侧接触阈值，单位 N。
GRIP_REQUIRES_BOTH = True    # True 表示必须左右两侧都接触才算夹住。
CLOSE_TIMEOUT_SECONDS = 5.0  # 触发闭合后这么久还没夹住，就重新张开等待。


# 检测到物体后慢慢加力，类似 01 的 gripping 状态。
FORCE_RAMP_STEP = 1
FORCE_RAMP_INTERVAL = 0.2
GRIP_SETTLE_SECONDS = 0.3    # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。

# 松开触发阈值。夹住后，法向或切向变化超过其中任意一个，就张开。
RELEASE_NORMAL_CHANGE = 0.03  # 左/右法向力相对夹住参考值的变化阈值，单位 N。
RELEASE_SHEAR_CHANGE = 0.03   # 左/右切向力相对夹住参考值的变化阈值，单位 N。

# 夹住后如果物体被拿走，法向和切向都降到这些阈值以下并持续一小段时间，也直接张开。
RELEASE_CONTACT_LOST_NORMAL_FORCE = 0.025  # 判断接触丢失的最大法向力，单位 N。
RELEASE_CONTACT_LOST_SHEAR_FORCE = 0.025   # 判断接触丢失的最大切向力，单位 N。
RELEASE_CONTACT_LOST_SECONDS = 0.15        # 接触丢失持续这么久后张开。

# 松开后至少等待多久重新进入等待触发，避免刚张开时马上再次触发。
REARM_SECONDS = 0.5

# 松开/超时张开后，必须等传感器读数低于这些阈值并稳定，才允许下一次触发。
# 这几个值应略小于 TRIGGER_NORMAL_FORCE / TRIGGER_SHEAR_FORCE。
RECOVER_NORMAL_FORCE = 0.035  # 张开恢复时允许重新触发的最大法向残余力，单位 N。
RECOVER_SHEAR_FORCE = 0.035   # 张开恢复时允许重新触发的最大切向残余力，单位 N。
RECOVER_STABLE_SECONDS = 0.25 # 力清空后需要连续稳定这么久，才回到 open_wait。


# 如果读取当前位置失败，保持当前位置时使用这个备用位置；None 表示不使用。
HOLD_FALLBACK_POS = None


# SDK 行为开关。
TRIGGER_FORCE_UPDATE = False
OPEN_AT_END = True
DRY_RUN = False


# 法向力标定：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},
        {"fnormal": 10000.0, "force_n": 0.377},
        {"fnormal": 30000.0, "force_n": 1.377},
        {"fnormal": 62000.0, "force_n": 2.377},
    ],
}


# 切向力标定：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},
    ],
}


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,
    "speed": SPEED,
    "accel": ACCEL,
    "decel": DECEL,

    "open_force": OPEN_FORCE,
    "close_force": CLOSE_FORCE,
    "grip_start_force": GRIP_START_FORCE,
    "hold_force": HOLD_FORCE,
    "initial_force": OPEN_FORCE,
    "force_min": FORCE_MIN,
    "force_max": FORCE_MAX,

    "control_hz": CONTROL_HZ,
    "baseline_seconds": BASELINE_SECONDS,
    "filter_alpha": FILTER_ALPHA,
    "shear_filter_alpha": SHEAR_FILTER_ALPHA,

    "trigger_normal_force": TRIGGER_NORMAL_FORCE,
    "trigger_shear_force": TRIGGER_SHEAR_FORCE,
    "trigger_normal_change": TRIGGER_NORMAL_CHANGE,
    "trigger_shear_change": TRIGGER_SHEAR_CHANGE,
    "grip_normal_force": GRIP_NORMAL_FORCE,
    "grip_requires_both": GRIP_REQUIRES_BOTH,
    "close_timeout_seconds": CLOSE_TIMEOUT_SECONDS,
    "force_ramp_step": FORCE_RAMP_STEP,
    "force_ramp_interval": FORCE_RAMP_INTERVAL,
    "grip_settle_seconds": GRIP_SETTLE_SECONDS,
    "release_normal_change": RELEASE_NORMAL_CHANGE,
    "release_shear_change": RELEASE_SHEAR_CHANGE,
    "release_contact_lost_normal_force": RELEASE_CONTACT_LOST_NORMAL_FORCE,
    "release_contact_lost_shear_force": RELEASE_CONTACT_LOST_SHEAR_FORCE,
    "release_contact_lost_seconds": RELEASE_CONTACT_LOST_SECONDS,
    "rearm_seconds": REARM_SECONDS,
    "recover_normal_force": RECOVER_NORMAL_FORCE,
    "recover_shear_force": RECOVER_SHEAR_FORCE,
    "recover_stable_seconds": RECOVER_STABLE_SECONDS,

    "hold_fallback_pos": HOLD_FALLBACK_POS,
    "trigger_force_update": TRIGGER_FORCE_UPDATE,
    "open_at_end": OPEN_AT_END,
    "dry_run": DRY_RUN,

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