import argparse
import copy
from dataclasses import dataclass, field

from gripper_control.calibration import (
    DEFAULT_NORMAL_FORCE_CALIBRATION,
    DEFAULT_SHEAR_FORCE_CALIBRATION,
)
from gripper_control.config import load_control_config
from gripper_control.paths import REPO_ROOT


DEFAULT_CONTROL_CONFIG = str(
    REPO_ROOT / "gripper_control_02" / "config" / "gripper_demo_02.py"
)


def _config_default(config, key, default):
    return config.get(key, config.get(key.replace("_", "-"), default))


@dataclass
class Demo02Config:
    control_config: str = DEFAULT_CONTROL_CONFIG

    video1: str = "0"
    video2: str = "1"
    rotation1: str = "./config/rotation_config_0.json"
    rotation2: str = "./config/rotation_config_1.json"
    sensor_config1: str = "./config/ddjx01.json"
    sensor_config2: str = "./config/ddjx01.json"
    sensor_fps: float = 30.0
    motion_threshold: float = 0.0
    sensor_cpu: bool = False

    port: str = "COM5"
    slave_id: int = 1
    baudrate: int = 115200
    serial_timeout: float = 0.2

    open_pos: int = 0
    close_pos: int = 9000
    speed: int = 25
    accel: int = 80
    decel: int = 80

    open_force: int = 10
    close_force: int = 10
    grip_start_force: int = 10
    hold_force: int = 15
    initial_force: int = 10
    force_min: int = 10
    force_max: int = 30

    control_hz: float = 30.0
    baseline_seconds: float = 1.0
    filter_alpha: float = 0.55
    shear_filter_alpha: float = 0.65

    trigger_normal_force: float = 0.04
    trigger_shear_force: float = 0.04
    trigger_normal_change: float = 0.02
    trigger_shear_change: float = 0.02
    grip_normal_force: float = 0.05
    grip_requires_both: bool = True
    close_timeout_seconds: float = 5.0
    force_ramp_step: int = 1
    force_ramp_interval: float = 0.2
    grip_settle_seconds: float = 0.3
    release_normal_change: float = 0.03
    release_shear_change: float = 0.03
    release_contact_lost_normal_force: float = 0.025
    release_contact_lost_shear_force: float = 0.025
    release_contact_lost_seconds: float = 0.15
    rearm_seconds: float = 0.5
    recover_normal_force: float = 0.035
    recover_shear_force: float = 0.035
    recover_stable_seconds: float = 0.25

    hold_fallback_pos: int | None = None
    trigger_force_update: bool = False
    open_at_end: bool = True
    dry_run: bool = False

    normal_force_calibration: dict = field(
        default_factory=lambda: copy.deepcopy(DEFAULT_NORMAL_FORCE_CALIBRATION)
    )
    shear_force_calibration: dict = field(
        default_factory=lambda: copy.deepcopy(DEFAULT_SHEAR_FORCE_CALIBRATION)
    )

    @classmethod
    def from_args(cls, argv=None):
        bootstrap = argparse.ArgumentParser(add_help=False)
        bootstrap.add_argument("--control-config", default=DEFAULT_CONTROL_CONFIG)
        bootstrap_args, _ = bootstrap.parse_known_args(argv)
        config_data = load_control_config(bootstrap_args.control_config)

        parser = argparse.ArgumentParser()
        add = parser.add_argument
        get = lambda key, default: _config_default(config_data, key, default)

        add("--control-config", type=str, default=bootstrap_args.control_config)
        add("--video1", "-v1", type=str, default=get("video1", cls.video1))
        add("--video2", "-v2", type=str, default=get("video2", cls.video2))
        add("--rotation1", "-r1", type=str, default=get("rotation1", cls.rotation1))
        add("--rotation2", "-r2", type=str, default=get("rotation2", cls.rotation2))
        add("--sensor-config1", type=str, default=get("sensor_config1", cls.sensor_config1))
        add("--sensor-config2", type=str, default=get("sensor_config2", cls.sensor_config2))
        add("--sensor-fps", type=float, default=get("sensor_fps", cls.sensor_fps))
        add("--motion-threshold", type=float, default=get("motion_threshold", cls.motion_threshold))
        add("--sensor-cpu", action="store_true", default=get("sensor_cpu", cls.sensor_cpu))

        add("--port", type=str, default=get("port", cls.port))
        add("--slave-id", type=int, default=get("slave_id", cls.slave_id))
        add("--baudrate", type=int, default=get("baudrate", cls.baudrate))
        add("--serial-timeout", type=float, default=get("serial_timeout", cls.serial_timeout))

        add("--open-pos", type=int, default=get("open_pos", cls.open_pos))
        add("--close-pos", type=int, default=get("close_pos", cls.close_pos))
        add("--speed", type=int, default=get("speed", cls.speed))
        add("--accel", type=int, default=get("accel", cls.accel))
        add("--decel", type=int, default=get("decel", cls.decel))

        add("--open-force", type=int, default=get("open_force", cls.open_force))
        add("--close-force", type=int, default=get("close_force", cls.close_force))
        add("--grip-start-force", type=int, default=get("grip_start_force", cls.grip_start_force))
        add("--hold-force", type=int, default=get("hold_force", cls.hold_force))
        add("--force-min", type=int, default=get("force_min", cls.force_min))
        add("--force-max", type=int, default=get("force_max", cls.force_max))

        add("--control-hz", type=float, default=get("control_hz", cls.control_hz))
        add("--baseline-seconds", type=float, default=get("baseline_seconds", cls.baseline_seconds))
        add("--filter-alpha", type=float, default=get("filter_alpha", cls.filter_alpha))
        add("--shear-filter-alpha", type=float, default=get("shear_filter_alpha", cls.shear_filter_alpha))

        add("--trigger-normal-force", type=float, default=get("trigger_normal_force", cls.trigger_normal_force))
        add("--trigger-shear-force", type=float, default=get("trigger_shear_force", cls.trigger_shear_force))
        add("--trigger-normal-change", type=float, default=get("trigger_normal_change", cls.trigger_normal_change))
        add("--trigger-shear-change", type=float, default=get("trigger_shear_change", cls.trigger_shear_change))
        add("--grip-normal-force", type=float, default=get("grip_normal_force", cls.grip_normal_force))
        add("--grip-requires-both", dest="grip_requires_both", action="store_true", default=get("grip_requires_both", cls.grip_requires_both))
        add("--allow-single-side-grip", dest="grip_requires_both", action="store_false")
        add("--close-timeout-seconds", type=float, default=get("close_timeout_seconds", cls.close_timeout_seconds))
        add("--force-ramp-step", type=int, default=get("force_ramp_step", cls.force_ramp_step))
        add("--force-ramp-interval", type=float, default=get("force_ramp_interval", cls.force_ramp_interval))
        add("--grip-settle-seconds", type=float, default=get("grip_settle_seconds", cls.grip_settle_seconds))
        add("--release-normal-change", type=float, default=get("release_normal_change", cls.release_normal_change))
        add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change))
        add("--release-contact-lost-normal-force", type=float, default=get("release_contact_lost_normal_force", cls.release_contact_lost_normal_force))
        add("--release-contact-lost-shear-force", type=float, default=get("release_contact_lost_shear_force", cls.release_contact_lost_shear_force))
        add("--release-contact-lost-seconds", type=float, default=get("release_contact_lost_seconds", cls.release_contact_lost_seconds))
        add("--rearm-seconds", type=float, default=get("rearm_seconds", cls.rearm_seconds))
        add("--recover-normal-force", type=float, default=get("recover_normal_force", cls.recover_normal_force))
        add("--recover-shear-force", type=float, default=get("recover_shear_force", cls.recover_shear_force))
        add("--recover-stable-seconds", type=float, default=get("recover_stable_seconds", cls.recover_stable_seconds))

        add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos))
        add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update))
        add("--open-at-end", action="store_true", default=get("open_at_end", cls.open_at_end))
        add("--dry-run", action="store_true", default=get("dry_run", cls.dry_run))

        args = parser.parse_args(argv)
        values = vars(args)
        values["initial_force"] = values["open_force"]
        values["normal_force_calibration"] = copy.deepcopy(
            config_data.get("normal_force_calibration", DEFAULT_NORMAL_FORCE_CALIBRATION)
        )
        values["shear_force_calibration"] = copy.deepcopy(
            config_data.get("shear_force_calibration", DEFAULT_SHEAR_FORCE_CALIBRATION)
        )
        return cls(**values)