"""
main
Pi 側アプリケーションのエントリーポイント
カメラ画像の送信と操舵量の受信・モーター制御を行う
"""

import time

from pi.camera.capture import CameraCapture
from pi.comm.zmq_client import PiZmqClient
from pi.motor.driver import MotorDriver


def main() -> None:
    """Pi 側のメインループを実行する"""
    camera = CameraCapture()
    zmq_client = PiZmqClient()
    motor = MotorDriver()

    try:
        camera.start()
        zmq_client.start()
        motor.start()
        print("Pi: カメラ・通信・モーターを開始")

        # FPS / RTT 計測用
        frame_count = 0
        fps_start = time.time()
        LOG_INTERVAL_SEC = 3.0

        while True:
            # カメラ画像を取得して送信
            frame = camera.capture()
            zmq_client.send_image(frame)
            frame_count += 1

            # 操舵量を受信してモーターに反映
            control = zmq_client.receive_control()
            if control is not None:
                throttle, steer = control
                motor.set_drive(throttle, steer)

            # タイムアウト時はモーター停止
            if zmq_client.is_timeout():
                motor.stop()

            # 定期的に FPS と RTT を表示
            elapsed = time.time() - fps_start
            if elapsed >= LOG_INTERVAL_SEC:
                fps = frame_count / elapsed
                rtt = zmq_client.last_rtt
                rtt_ms = (
                    f"{rtt * 1000:.1f}ms"
                    if rtt is not None else "---"
                )
                print(
                    f"Pi: カメラ FPS={fps:.1f}"
                    f"  RTT={rtt_ms}"
                )
                frame_count = 0
                fps_start = time.time()

            time.sleep(0.01)

    except KeyboardInterrupt:
        print("\nPi: 終了")
    finally:
        motor.cleanup()
        camera.stop()
        zmq_client.stop()


if __name__ == "__main__":
    main()