"""夹爪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
RELEASE_OPEN_SPEED = 50    # 检测到人取物/力变化后，张开使用的速度。
RELEASE_OPEN_ACCEL = 120   # release 张开的加速度。
RELEASE_OPEN_DECEL = 120   # release 张开的减速度。


# 夹爪力度百分比。
OPEN_FORCE = 15           # 张开时使用的力。
CLOSE_FORCE = 15          # 单侧触发后闭合时使用的小力。
GRIP_START_FORCE = 10     # 闭合检测到物体后，先切到这个低力保持。
HOLD_FORCE = 15           # 基础夹持力；轻物体默认慢慢加到这个力。
FORCE_MIN = 10
FORCE_MAX = 80            # 允许自适应加力的最大总上限；重物夹不住时可以适当加大。


# 控制循环频率。越高检测释放越快。
CONTROL_HZ = 30.0


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

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


# 等待触发：任意一侧超过这些阈值，就开始闭合。
TRIGGER_NORMAL_FORCE = 0.1  # 单侧法向力触发阈值，单位 N。
TRIGGER_SHEAR_FORCE = 0.1   # 单侧切向力触发阈值，单位 N。
TRIGGER_CLEAR_STABLE_SECONDS = 0.2  # 张开恢复后，必须先看到触发力清掉并稳定这么久，才允许下一次扫过触发。


# 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。
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.1
GRIP_SETTLE_SECONDS = 0.1    # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。

# 根据切向力估计物体负载，并慢慢补夹紧力。
# 逻辑：切向力越大，说明物体越重或越容易下滑，目标夹紧力越高。
ADAPTIVE_GRIP_ENABLED = True        # 是否启用切向力自适应加力。
ADAPTIVE_SHEAR_START_FORCE = 0.10   # 切向力低于这个值时，认为是轻物体，只用 ADAPTIVE_FORCE_MIN。
ADAPTIVE_SHEAR_FULL_FORCE = 0.60    # 切向力达到这个值时，认为负载较大，目标力加到 ADAPTIVE_FORCE_MAX。
ADAPTIVE_FORCE_MIN = 15             # 自适应夹持的最小目标力，通常等于 HOLD_FORCE。
ADAPTIVE_FORCE_MAX = 45             # 自适应夹持的最大目标力；重物夹不住可以加大，但不要超过 FORCE_MAX。
ADAPTIVE_FORCE_STEP = 1             # 自适应加力每次增加多少 force_pct。
ADAPTIVE_FORCE_INTERVAL = 0.15      # 自适应加力两次之间的间隔，单位秒；越大加力越柔和。
RELEASE_ARM_DELAY_SECONDS = 0.8     # 自适应加力停止后，等待这么久再启用“人取物松开”检测。

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

# 张开到位或恢复条件成立后，至少休息多久才重新进入等待触发。
# 休息期间即使有物体扫过，也不会触发闭合。
REARM_SECONDS = 0.5

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

# 松开/超时张开后，如果夹爪已经回到 OPEN_POS 初始点位，也允许重新触发。
# 这样传感器上还有一点残余力时，不会一直卡在 open_recover。
OPEN_POSITION_RECOVER_ENABLED = True
OPEN_POSITION_TOLERANCE = 100         # 当前位置和 OPEN_POS 的差值小于它，就认为回到初始点位。
OPEN_POSITION_STABLE_SECONDS = 0.1    # 回到初始点位后稳定这么久，才回到 open_wait。
OPEN_POSITION_CHECK_INTERVAL = 0.1    # open_recover 状态下读取夹爪位置的间隔，单位秒。


# 如果读取当前位置失败，保持当前位置时使用这个备用位置；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},
    ],
}


# 切向力标定：FSHEARX / FSHEARY 原始分量 -> N。
# X/Y 分量分别按同一条曲线标定；控制逻辑再使用标定后分量的合力。
SHEAR_FORCE_CALIBRATION = {
    "enabled": True,
    "method": "piecewise_linear",
    "input": "components",
    "signed": True,
    "extrapolate": True,
    "clamp_output_min": 0.0,
    "points": [
        {"raw": 1000.0, "force_n": 0.0},
        {"raw": 36000.0, "force_n": 1.19},
        {"raw": 68000.0, "force_n": 2.06},
    ],
}


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,
    "release_open_speed": RELEASE_OPEN_SPEED,
    "release_open_accel": RELEASE_OPEN_ACCEL,
    "release_open_decel": RELEASE_OPEN_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_clear_stable_seconds": TRIGGER_CLEAR_STABLE_SECONDS,
    "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,
    "adaptive_grip_enabled": ADAPTIVE_GRIP_ENABLED,
    "adaptive_shear_start_force": ADAPTIVE_SHEAR_START_FORCE,
    "adaptive_shear_full_force": ADAPTIVE_SHEAR_FULL_FORCE,
    "adaptive_force_min": ADAPTIVE_FORCE_MIN,
    "adaptive_force_max": ADAPTIVE_FORCE_MAX,
    "adaptive_force_step": ADAPTIVE_FORCE_STEP,
    "adaptive_force_interval": ADAPTIVE_FORCE_INTERVAL,
    "release_arm_delay_seconds": RELEASE_ARM_DELAY_SECONDS,
    "release_normal_change": RELEASE_NORMAL_CHANGE,
    "release_shear_change": RELEASE_SHEAR_CHANGE,
    "rearm_seconds": REARM_SECONDS,
    "recover_normal_force": RECOVER_NORMAL_FORCE,
    "recover_shear_force": RECOVER_SHEAR_FORCE,
    "recover_stable_seconds": RECOVER_STABLE_SECONDS,
    "open_position_recover_enabled": OPEN_POSITION_RECOVER_ENABLED,
    "open_position_tolerance": OPEN_POSITION_TOLERANCE,
    "open_position_stable_seconds": OPEN_POSITION_STABLE_SECONDS,
    "open_position_check_interval": OPEN_POSITION_CHECK_INTERVAL,

    "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,
}
