# -*- coding: utf-8 -*-
"""
nao_video_server.py - RUNS ON THE ROBOT, under its own Python 2.7.

Opens a qi session to NAOqi over localhost (zero network hop - it's
the same box) and serves an MJPEG video stream over plain HTTP. Only
compressed JPEG bytes ever cross the phone tunnel, instead of raw RGB
frames.

Needs the SDK's own env vars, since the bindings aren't on a login
shell's default path:

    export PYTHONPATH=/opt/aldebaran/lib/python2.7/site-packages
    export LD_LIBRARY_PATH=/opt/aldebaran/lib
    python2 nao_video_server.py

No Flask here on purpose - it's very unlikely to be installed on this
locked-down embedded OS and there's no easy way to pip install it
without network access on the robot. This uses only the stdlib
(BaseHTTPServer/SocketServer) plus qi and cv2, which we've confirmed
are already present.
"""
import json
import time
import threading

import BaseHTTPServer
import SocketServer

import cv2
import numpy as np
import qi

# ---- tunables ----
NAOQI_PORT = 9561          # local qi session port (confirmed working)
HTTP_PORT = 8000           # forward this through the phone tunnel
CAMERA_RESOLUTION = 1      # 0=kQQVGA(160x120) 1=kQVGA(320x240) 2=kVGA(640x480)
CAMERA_COLORSPACE = 11     # kRGBColorSpace
CAMERA_FPS = 20
JPEG_QUALITY = 60          # 1-100 - tune this first if the link is still tight


class FrameGrabber(object):
    """Background thread: keeps the single most recent JPEG-encoded
    frame available. Same pattern as naowalk.py's own _video_loop -
    a stalled capture never blocks a client's HTTP request."""

    def __init__(self, session):
        self.video = session.service("ALVideoDevice")
        self.client = self.video.subscribeCamera(
            "MjpegServer", 0, CAMERA_RESOLUTION, CAMERA_COLORSPACE, CAMERA_FPS)
        self._lock = threading.Lock()
        self._jpeg = None
        self._stop = threading.Event()
        self._thread = threading.Thread(target=self._loop)
        self._thread.daemon = True
        self._thread.start()

    def _loop(self):
        while not self._stop.is_set():
            try:
                image = self.video.getImageRemote(self.client)
                if not image or len(image) < 7:
                    continue
                w, h = image[0], image[1]
                arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
                bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
                ok, buf = cv2.imencode(".jpg", bgr, [cv2.IMWRITE_JPEG_QUALITY, JPEG_QUALITY])
                if ok:
                    with self._lock:
                        self._jpeg = buf.tobytes()
            except Exception as e:
                print("frame grab error: %s" % e)
                time.sleep(0.2)

    def latest(self):
        with self._lock:
            return self._jpeg

    def close(self):
        self._stop.set()
        try:
            self.video.unsubscribe(self.client)
        except Exception:
            pass


class Handler(BaseHTTPServer.BaseHTTPRequestHandler):
    def log_message(self, fmt, *args):
        pass  # keep the robot's console quiet

    def do_GET(self):
        if self.path == "/video":
            self._serve_video()
        elif self.path == "/battery":
            self._serve_battery()
        elif self.path == "/health":
            self._ok()
        else:
            self.send_error(404)

    def do_POST(self):
        length = int(self.headers.getheader("content-length", 0))
        raw = self.rfile.read(length) if length else "{}"
        try:
            payload = json.loads(raw)
        except ValueError:
            payload = {}

        if self.path == "/move":
            x = float(payload.get("x", 0.0))
            y = float(payload.get("y", 0.0))
            theta = float(payload.get("theta", 0.0))
            self.server.motion.moveToward(x, y, theta)
            self._ok()
        elif self.path == "/stop":
            self.server.motion.stopMove()
            self._ok()
        elif self.path == "/say":
            self.server.tts.say(str(payload.get("text", "")))
            self._ok()
        else:
            self.send_error(404)

    def _ok(self):
        self.send_response(200)
        self.send_header("Content-Type", "application/json")
        self.end_headers()
        self.wfile.write(b'{"ok": true}')

    def _serve_battery(self):
        try:
            pct = self.server.battery.getBatteryCharge() if self.server.battery else -1
        except Exception:
            pct = -1
        self.send_response(200)
        self.send_header("Content-Type", "application/json")
        self.end_headers()
        self.wfile.write(json.dumps({"battery": pct}).encode())

    def _serve_video(self):
        self.send_response(200)
        self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=frame")
        self.end_headers()
        try:
            while True:
                jpeg = self.server.grabber.latest()
                if jpeg is not None:
                    self.wfile.write(b"--frame\r\n")
                    self.wfile.write(b"Content-Type: image/jpeg\r\n")
                    self.wfile.write(b"Content-Length: %d\r\n\r\n" % len(jpeg))
                    self.wfile.write(jpeg)
                    self.wfile.write(b"\r\n")
                time.sleep(1.0 / CAMERA_FPS)
        except Exception:
            pass  # client disconnected - totally normal, not an error


class ThreadingHTTPServer(SocketServer.ThreadingMixIn, BaseHTTPServer.HTTPServer):
    daemon_threads = True
    allow_reuse_address = True


def main():
    session = qi.Session()
    session.connect("tcp://127.0.0.1:%d" % NAOQI_PORT)
    print("connected to local NAOqi on port %d" % NAOQI_PORT)

    server = ThreadingHTTPServer(("0.0.0.0", HTTP_PORT), Handler)
    server.grabber = FrameGrabber(session)
    server.motion = session.service("ALMotion")
    server.tts = session.service("ALTextToSpeech")
    try:
        server.battery = session.service("ALBattery")
    except Exception:
        server.battery = None

    print("serving on :%d  (/video, /move, /stop, /say, /battery, /health)" % HTTP_PORT)
    try:
        server.serve_forever()
    except KeyboardInterrupt:
        pass
    finally:
        server.grabber.close()


if __name__ == "__main__":
    main()