import time

from .filters import clamp


class TimedCycleGripperController:
    def __init__(self, config, reader, gripper, feedback_processor):
        self.config = config
        self.reader = reader
        self.gripper = gripper
        self.feedback_processor = feedback_processor
        self.normal_unit = feedback_processor.normal_converter.unit
        self.shear_unit = feedback_processor.shear_converter.unit

    def run(self):
        config = self.config
        self.reader.start()

        try:
            self._print_thresholds()
            print(
                f"Calibrating baseline for {config.baseline_seconds:.2f}s. "
                "Keep sensors unloaded."
            )
            baseline = self.feedback_processor.calibrate_baseline(self.reader)
            print(
                f"baseline normal: left={baseline.left_normal:.4f}{self.normal_unit}, "
                f"right={baseline.right_normal:.4f}{self.normal_unit}; "
                f"shear: left={baseline.left_shear:.4f}{self.shear_unit}, "
                f"right={baseline.right_shear:.4f}{self.shear_unit}"
            )

            self.gripper.connect()

            cycle_index = 1
            while config.cycles <= 0 or cycle_index <= config.cycles:
                open_force = int(clamp(
                    config.initial_force,
                    config.force_min,
                    config.force_max,
                ))
                self._run_open_phase(open_force, cycle_index)
                result = self._run_close_phase(cycle_index)

                if result in ("emergency", "released"):
                    time.sleep(config.open_seconds)
                if result == "gripped":
                    print("object is gripped; timed open/close loop stopped")
                    break
                cycle_index += 1

        except KeyboardInterrupt:
            print("\nStopping timed cycle control.")
        finally:
            self.gripper.final_open_if_needed()
            self.reader.stop()

    def _print_thresholds(self):
        config = self.config
        print(
            "Normal force thresholds: "
            f"both_contact={config.both_contact_force:.3f}{self.normal_unit}, "
            f"object={config.object_force:.3f}{self.normal_unit}, "
            f"emergency={config.emergency_touch_force:.3f}{self.normal_unit}, "
            f"requires_both={int(config.object_requires_both_contact)}, "
            f"empty_release={config.empty_release_seconds:.2f}s"
        )
        print(
            "Shear force thresholds: "
            f"hold_min={config.shear_hold_force:.3f}{self.shear_unit}, "
            f"stable_delta={config.shear_stable_delta:.3f}{self.shear_unit}, "
            f"stable_seconds={config.shear_stable_seconds:.2f}s, "
            f"release_change={config.release_shear_change:.3f}{self.shear_unit}"
        )

    def _run_open_phase(self, force_pct, cycle_index):
        print(f"\ncycle {cycle_index}: open phase")
        self.gripper.open(force_pct, "open")

        deadline = time.perf_counter() + self.config.open_seconds
        while time.perf_counter() < deadline:
            self.reader.update()
            time.sleep(0.02)

    def _run_close_phase(self, cycle_index):
        config = self.config
        print(f"cycle {cycle_index}: close phase")
        self.feedback_processor.reset_filters()

        force_pct = int(clamp(config.initial_force, config.force_min, config.force_max))
        self.gripper.close(force_pct, "close")

        interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.1
        phase_start = time.perf_counter()
        last_close_command_time = phase_start
        last_force_ramp_time = phase_start
        state = "closing"
        hold_pos = None
        previous_shear = None
        hold_start_time = None
        hold_shear_reference = None
        release_monitor_armed = False
        shear_stable_since = None
        empty_since = None
        gripped = False
        deadline = time.perf_counter() + config.close_seconds

        while True:
            tick_start = time.perf_counter()
            now = time.perf_counter()
            if state == "closing" and now >= deadline:
                print("close phase finished without object; continue timed open/close cycle")
                return "no_object"

            feedback = self.feedback_processor.read(self.reader, timeout=0.3)
            if feedback is None:
                phase_time = time.perf_counter() - phase_start
                print(f"t={phase_time:5.2f}s action=timed-close waiting for sensor samples")
                time.sleep(0.05)
                continue

            has_previous_shear = previous_shear is not None
            shear_delta = 0.0 if not has_previous_shear else feedback.max_shear - previous_shear
            shear_abs_delta = abs(shear_delta)
            previous_shear = feedback.max_shear

            both_contact = (
                feedback.left_normal >= config.both_contact_force
                and feedback.right_normal >= config.both_contact_force
            )
            normal_force_detected = feedback.max_normal >= config.object_force
            contact_confirmed = (
                normal_force_detected
                and (both_contact or not config.object_requires_both_contact)
            )
            object_detected = contact_confirmed
            grip_contact = (
                both_contact
                if config.object_requires_both_contact
                else feedback.max_normal >= config.both_contact_force
            )

            if state in ("gripping", "hold_check") and not grip_contact:
                if empty_since is None:
                    empty_since = now
                elif now - empty_since >= config.empty_release_seconds:
                    force_pct = self.gripper.set_force(config.force_min)
                    print(
                        f"L={feedback.left_normal:8.3f}{self.normal_unit} "
                        f"R={feedback.right_normal:8.3f}{self.normal_unit} "
                        f"min={feedback.min_normal:8.3f}{self.normal_unit} "
                        f"max={feedback.max_normal:8.3f}{self.normal_unit} "
                        f"force_pct={force_pct:3d} action=empty-grip-resume-cycle"
                    )
                    return "no_object"
            else:
                empty_since = None

            if feedback.max_normal >= config.emergency_touch_force:
                if config.enable_emergency_open:
                    force_pct = self.gripper.set_force(config.force_min)
                    self.gripper.open(force_pct, "emergency-open")
                    print(
                        f"L={feedback.left_normal:8.3f}{self.normal_unit} "
                        f"R={feedback.right_normal:8.3f}{self.normal_unit} "
                        f"max={feedback.max_normal:8.3f}{self.normal_unit} "
                        f"force_pct={force_pct:3d} action=emergency-open"
                    )
                    return "emergency"
                hold_pos = self.gripper.hold_current_position(force_pct)
                state = "hold_check"
                gripped = True
                if hold_start_time is None:
                    hold_start_time = now
                action = "overforce-hold"

            if state == "closing":
                if contact_confirmed:
                    state = "gripping"
                    gripped = True
                    force_pct = int(clamp(
                        config.grip_start_force,
                        config.force_min,
                        config.force_max,
                    ))
                    force_pct = self.gripper.set_force(force_pct)
                    hold_pos = self.gripper.hold_current_position(force_pct)
                    last_force_ramp_time = now
                    shear_stable_since = None
                    action = "object-detected-low-force"
                    print(
                        "object detected; timed open/close cycle stopped, "
                        f"grip force reset to {force_pct}"
                    )
                else:
                    action = "closing"
                    if (
                        config.close_command_interval > 0
                        and now - last_close_command_time >= config.close_command_interval
                    ):
                        self.gripper.close(force_pct, "close-continue")
                        last_close_command_time = now

            elif state == "gripping":
                shear_is_high_enough = feedback.max_shear >= config.shear_hold_force
                shear_is_stable = (
                    shear_is_high_enough
                    and has_previous_shear
                    and shear_abs_delta <= config.shear_stable_delta
                )

                if shear_is_stable:
                    if shear_stable_since is None:
                        shear_stable_since = now
                else:
                    shear_stable_since = None

                if (
                    shear_stable_since is not None
                    and now - shear_stable_since >= config.shear_stable_seconds
                ):
                    state = "hold_check"
                    hold_start_time = now
                    hold_shear_reference = None
                    release_monitor_armed = False
                    hold_pos = self.gripper.hold_current_position(force_pct)
                    action = "shear-stable-hold"
                elif shear_is_high_enough:
                    action = "wait-shear-stable"
                elif (
                    now - last_force_ramp_time >= config.force_ramp_interval
                    and force_pct < config.force_max
                ):
                    force_pct = int(clamp(
                        force_pct + config.force_ramp_step,
                        config.force_min,
                        config.force_max,
                    ))
                    force_pct = self.gripper.set_force(force_pct)
                    last_force_ramp_time = now
                    action = "grip-ramp-up"
                else:
                    action = "grip-hold"

            elif state == "hold_check":
                hold_elapsed = now - hold_start_time
                if not release_monitor_armed and hold_elapsed >= config.human_hold_seconds:
                    hold_shear_reference = feedback.max_shear
                    release_monitor_armed = True
                    action = "hold-check-armed"
                elif release_monitor_armed:
                    hold_shear_change = abs(feedback.max_shear - hold_shear_reference)
                    if hold_shear_change >= config.release_shear_change:
                        if config.enable_human_release:
                            force_pct = self.gripper.set_force(config.force_min)
                            self.gripper.open(force_pct, "human-release-open")
                            print(
                                f"L={feedback.left_normal:8.3f}{self.normal_unit} "
                                f"R={feedback.right_normal:8.3f}{self.normal_unit} "
                                f"shear={feedback.max_shear:8.3f}{self.shear_unit} "
                                f"hold_change={hold_shear_change:8.3f}{self.shear_unit} "
                                f"force_pct={force_pct:3d} action=human-release-open"
                            )
                            return "released"
                        hold_shear_reference = feedback.max_shear
                        action = "hold-check-change-ignored"
                    else:
                        action = "hold-check"
                else:
                    action = "hold-check"

            self._log_tick(
                phase_start,
                feedback,
                shear_delta,
                both_contact,
                object_detected,
                grip_contact,
                empty_since,
                shear_stable_since,
                state,
                gripped,
                hold_pos,
                force_pct,
                action,
            )

            sleep_time = interval - (time.perf_counter() - tick_start)
            if sleep_time > 0:
                time.sleep(sleep_time)

    def _log_tick(
        self,
        phase_start,
        feedback,
        shear_delta,
        both_contact,
        object_detected,
        grip_contact,
        empty_since,
        shear_stable_since,
        state,
        gripped,
        hold_pos,
        force_pct,
        action,
    ):
        now = time.perf_counter()
        phase_time = now - phase_start
        print(
            f"t={phase_time:5.2f}s "
            f"L={feedback.left_normal:8.3f}{self.normal_unit} "
            f"R={feedback.right_normal:8.3f}{self.normal_unit} "
            f"min={feedback.min_normal:8.3f}{self.normal_unit} "
            f"max={feedback.max_normal:8.3f}{self.normal_unit} "
            f"diff={feedback.normal_diff:8.3f}{self.normal_unit} "
            f"both={int(both_contact)} object={int(object_detected)} "
            f"grip_contact={int(grip_contact)} "
            f"empty_for={0.0 if empty_since is None else now - empty_since:4.1f}s "
            f"shear={feedback.max_shear:8.3f}{self.shear_unit} "
            f"d_shear={shear_delta:8.3f}{self.shear_unit} "
            f"shear_stable={int(shear_stable_since is not None)} "
            f"state={state} gripped={int(gripped)} hold_pos={hold_pos} "
            f"force_pct={force_pct:3d} action={action}"
        )

