import serial
import time

class RobotDriver:
    def __init__(self, port='COM9', baud=115200, ack=True, timeout=0.25):
        self.ack = ack
        self.timeout = timeout
        self.ser = serial.Serial(port, baud, timeout=timeout, write_timeout=0.5)
        time.sleep(2.0)
        self.ser.reset_input_buffer()
        print(f"[Robot] Connected on {port} @ {baud}")

    def _readline(self):
        return self.ser.readline().decode("ascii", errors="replace").strip()

    def _send(self, cmd, expect_ok=True):
        packet = f"{cmd}\r\n".encode("ascii")
        if not self.ack or not expect_ok:
            self.ser.write(packet)
            self.ser.flush()
            return None

        MAX_RETRIES = 3
        for attempt in range(MAX_RETRIES):
            self.ser.write(packet)
            self.ser.flush()
            deadline = time.time() + 0.3
            last_line = ""
            while time.time() < deadline:
                line = self._readline()
                if not line:
                    continue
                last_line = line
                if line.startswith("OK:") or line.startswith("READY:"):
                    return line
                if line.startswith("ERR:"):
                    raise RuntimeError(f"MCU rejected {cmd}: {line}")
            if attempt < MAX_RETRIES - 1:
                time.sleep(0.1)
        raise TimeoutError(f"No ACK for {cmd} after {MAX_RETRIES} attempts")

    def ping(self):
        return self._send("PING")

    def motor_open(self):
        return self._send("M:OPEN")

    def motor_close(self):
        return self._send("M:CLOSE")

    def motor_stop(self):
        return self._send("M:STOP")

    def set_servo(self, servo_id, angle):
        angle = max(0, min(180, int(angle)))
        return self._send(f"S{servo_id}:{angle}")

    def set_config(self, mode):
        return self._send(f"CFG:{int(mode)}")

    def close(self):
        try:
            if self.ser and self.ser.is_open:
                self.motor_stop()
        except Exception:
            pass
        finally:
            if self.ser:
                self.ser.close()