from .filters import clamp
from .paths import ensure_project_paths


class GripperClient:
    def __init__(self, config):
        self.config = config
        self.motor = None

    def connect(self):
        if self.config.dry_run:
            return self

        ensure_project_paths()
        from changingtek_p_rtu_Servo import MotorController

        self.motor = MotorController(
            self.config.port,
            self.config.slave_id,
            baudrate=self.config.baudrate,
            timeout=self.config.serial_timeout,
        )
        return self

    def move(self, position, force_pct, label):
        config = self.config
        if config.dry_run:
            print(
                f"[dry-run] {label}: temp_move "
                f"position={position}, speed={config.speed}, force={force_pct}, "
                f"accel={config.accel}, decel={config.decel}"
            )
            return

        self.motor.temp_move(
            position_mm=int(position),
            speed_pct=int(config.speed),
            force_pct=int(force_pct),
            accel=int(config.accel),
            decel=int(config.decel),
            trigger=True,
        )

    def open(self, force_pct, label="open"):
        self.move(self.config.open_pos, force_pct, label)

    def close(self, force_pct, label="close"):
        self.move(self.config.close_pos, force_pct, label)

    def set_force(self, force_pct):
        config = self.config
        force_pct = int(clamp(force_pct, config.force_min, config.force_max))
        if config.dry_run:
            print(f"[dry-run] set_target_force {force_pct}")
            return force_pct

        self.motor.set_target_force(force_pct)
        if config.trigger_force_update:
            self.motor.trigger_motion()
        return force_pct

    def read_position(self):
        if self.config.dry_run:
            return None
        try:
            return self.motor.read_real_position()
        except Exception as exc:
            print(f"read_real_position failed: {exc}")
            return None

    def hold_current_position(self, force_pct):
        current_pos = self.read_position()
        if current_pos is None:
            current_pos = self.config.hold_fallback_pos

        if current_pos is None:
            self.set_force(force_pct)
            self._stop_by_speed()
            return None

        self.move(int(current_pos), force_pct, "clamp-hold")
        return int(current_pos)

    def final_open_if_needed(self):
        if not self.config.open_at_end:
            return
        final_force = int(clamp(
            self.config.initial_force,
            self.config.force_min,
            self.config.force_max,
        ))
        self.open(final_force, "open-at-end")

    def _stop_by_speed(self):
        if self.config.dry_run:
            return
        try:
            self.motor.set_target_speed(0)
            self.motor.trigger_motion()
        except Exception as exc:
            print(f"stop by speed=0 failed: {exc}")