#!/usr/bin/env python3
"""Camera live view for testing (lib/camview.dart). Development tool, not the vision service.

Streams the USB camera's own MJPEG frames, unmodified, to any client on 7710/tcp:

  server → client, per frame:  [u32 length][f64 capture time, unix s][u32 seq][JPEG]
               per text:       [u32 0][f64 time][u32 text length][UTF-8 text]
  client → server, lines:      "exposure auto" | "exposure <100 µs units>" | "gain <0-240>"
                               → one text reply, "ok <state>" or "err <reason>"

Texts also carry availability: "camera unavailable" when no frame has come for
UNAVAILABLE_S (none yet, or capture stopped), "camera ok" when frames resume.

Latest-wins: each client gets the newest frame when it is ready for one, never a
queue of old ones. Sockets are paced (SO_MAX_PACING_RATE, project-yukart decision
28) because unpaced bursts stall the USB link to the tablet for seconds.

Run on the Orin: python3 camview_server.py [--size 1280x720] [--fps 30]
"""

import argparse
import ipaddress
import socket
import struct
import subprocess
import threading
import time

SO_MAX_PACING_RATE = 47
CONTROLS = {"exposure", "gain"}
UNAVAILABLE_S = 3.0


class Latest:
    """The newest frame, and a condition to wait for the next one."""

    def __init__(self):
        self.cond = threading.Condition()
        self.frame = None  # (seq, capture_time, jpeg)

    def put(self, seq, t, jpeg):
        with self.cond:
            self.frame = (seq, t, jpeg)
            self.cond.notify_all()

    def after(self, seq, timeout):
        with self.cond:
            self.cond.wait_for(lambda: self.frame and self.frame[0] > seq, timeout)
            return self.frame


def capture(dev, w, h, fps, latest):
    # multipartmux frames each JPEG with a Content-Length header, so no JPEG parsing is needed.
    cmd = ["gst-launch-1.0", "-q", "v4l2src", f"device={dev}", "!",
           f"image/jpeg,width={w},height={h},framerate={fps}/1", "!",
           "multipartmux", "boundary=yukartframe", "!", "fdsink", "fd=1"]
    seq = 0
    while True:
        p = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
        out = p.stdout
        try:
            while True:
                length = None
                while True:  # headers up to the blank line
                    line = out.readline()
                    if not line:
                        raise EOFError
                    line = line.strip()
                    if line.lower().startswith(b"content-length:"):
                        length = int(line.split(b":")[1])
                    elif not line and length is not None:
                        break
                jpeg = out.read(length)
                if len(jpeg) < length:
                    raise EOFError
                seq += 1
                latest.put(seq, time.time(), jpeg)
        except EOFError:
            print("capture ended; restarting in 1 s", flush=True)
        p.kill()
        p.wait()
        time.sleep(1)


def v4l2(dev, *args):
    r = subprocess.run(["v4l2-ctl", "-d", dev, *args], capture_output=True, text=True)
    return r.returncode == 0, (r.stdout + r.stderr).strip()


def control(dev, line):
    parts = line.split()
    if len(parts) != 2 or parts[0] not in CONTROLS:
        return "err expected: exposure auto|<n> or gain <n>"
    name, value = parts
    if name == "exposure" and value == "auto":
        ok, msg = v4l2(dev, "-c", "auto_exposure=3")
    elif name == "exposure" and value.isdigit():
        ok, msg = v4l2(dev, "-c", "auto_exposure=1", "-c", f"exposure_time_absolute={value}")
    elif name == "gain" and value.isdigit():
        ok, msg = v4l2(dev, "-c", f"gain={value}")
    else:
        return "err bad value"
    _, state = v4l2(dev, "-C", "auto_exposure,exposure_time_absolute,gain")
    return ("ok " if ok else "err ") + (msg or " ".join(state.split()))


def serve_client(conn, peer, dev, latest, pace):
    conn.setsockopt(socket.SOL_SOCKET, SO_MAX_PACING_RATE, pace)
    conn.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 256 * 1024)  # bound what can queue behind a frame
    conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
    alive = threading.Event()
    alive.set()
    send = threading.Lock()  # frames and texts share the socket; never interleave them

    def send_text(text):
        # Texts go in-band as a frame with length 0 and the text after the header.
        msg = text.encode()
        with send:
            conn.sendall(struct.pack("!IdI", 0, time.time(), len(msg)) + msg)

    def reader():
        buf = b""
        try:
            while alive.is_set():
                b = conn.recv(1024)
                if not b:
                    break
                buf += b
                while b"\n" in buf:
                    line, buf = buf.split(b"\n", 1)
                    reply = control(dev, line.decode(errors="replace").strip())
                    print(f"{peer}: {line.decode(errors='replace').strip()} -> {reply}", flush=True)
                    send_text(reply)
        except OSError:
            pass
        alive.clear()

    threading.Thread(target=reader, daemon=True).start()
    seq = 0
    sent = 0
    available = None  # unknown until the first frame or the first timeout
    try:
        while alive.is_set():
            f = latest.after(seq, UNAVAILABLE_S)
            if not f or f[0] <= seq:
                if available is not False:
                    available = False
                    send_text("camera unavailable")
                continue
            if available is not True:
                available = True
                send_text("camera ok")
            seq, t, jpeg = f
            with send:
                conn.sendall(struct.pack("!IdI", len(jpeg), t, seq) + jpeg)
            sent += 1
    except OSError:
        pass
    alive.clear()
    print(f"{peer}: gone after {sent} frames", flush=True)
    conn.close()


def allowed(ip):
    a = ipaddress.ip_address(ip)
    return a.is_loopback or a.is_private or a in ipaddress.ip_network("100.64.0.0/10")


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--dev", default="/dev/v4l/by-id/usb-USB_Camera_1080P_USB_Camera_1080P_01.00.00-video-index0")
    ap.add_argument("--size", default="1280x720")
    ap.add_argument("--fps", type=int, default=30)
    ap.add_argument("--port", type=int, default=7710)
    ap.add_argument("--pace", type=int, default=4_000_000, help="bytes/s per client")
    a = ap.parse_args()
    w, h = map(int, a.size.split("x"))
    latest = Latest()
    threading.Thread(target=capture, args=(a.dev, w, h, a.fps, latest), daemon=True).start()
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    s.bind(("0.0.0.0", a.port))
    s.listen(4)
    print(f"camview: {a.dev} {w}x{h}@{a.fps} MJPEG on tcp {a.port}, paced {a.pace / 1e6} MB/s per client", flush=True)
    while True:
        conn, addr = s.accept()
        if not allowed(addr[0]):
            conn.close()
            continue
        print(f"{addr[0]}: connected", flush=True)
        threading.Thread(target=serve_client, args=(conn, addr[0], a.dev, latest, a.pace), daemon=True).start()


if __name__ == "__main__":
    main()