import sys
import solaros
from solaros import camera, emotion, pipeline, time, tui

SOURCE = "camera0"
SCORE = 60
MIRROR_X = True
POLL_MS = 50
SOUND_GAP_MS = 8000
LOST_MS = 1600


def face_slot(inventory):
    for name in ("himax-face-detect-96", "himax-face-detect-192"):
        for slot in inventory["slots"]:
            record = slot.get("record")
            if record and record.get("state") == "programmed" and record.get("id") == name:
                return slot["slot"]
    raise RuntimeError("No programmed Himax face detector")


def clamp(value):
    return max(-1.0, min(1.0, value))


def main(seconds=0):
    inventory = pipeline.info("himax", SOURCE, "slots")
    slot = face_slot(inventory)
    previous = inventory["active_slot"]
    face = None
    detector = None
    sequence = 0
    gaze_x = gaze_y = target_x = target_y = 0.0
    sent_x = sent_y = 2.0
    streak = 0
    present = False
    last_seen = last_frame = 0
    period = 500.0
    last_sound = -SOUND_GAP_MS
    started = time.uptime_ms()
    print("Himax face detector slot", slot, "- wheel press or Ctrl+] exits")
    try:
        detector = pipeline.start(SOURCE, "himax", slot, 0, 100)
        sensor = camera.status()
        width, height = sensor["width"], sensor["height"]
        face = emotion.open(sound=True, fps=20, motion=True, fullscreen=True)
        face.set("bored", transition_ms=0)
        while not solaros.should_exit():
            now = time.uptime_ms()
            if tui.getch() in (10, 13, 27, 113):
                break
            if seconds and now - started >= seconds * 1000:
                break
            record = pipeline.result(detector, sequence)
            if record is not None:
                sequence = record["sequence"]
                if last_frame:
                    period = period * 0.75 + (now - last_frame) * 0.25
                last_frame = now
                result = record["result"]
                boxes = [b for b in result.get("boxes", ())
                         if len(b) >= 6 and b[4] >= SCORE and b[5] == 0]
                if boxes:
                    box = max(boxes, key=lambda b: b[2] * b[3])
                    streak += 1
                    last_seen = now
                    if streak >= 2:
                        if not present:
                            face.set("happy", transition_ms=150)
                        if not present or now - last_sound >= SOUND_GAP_MS:
                            face.react("happy", hold_ms=750, transition_ms=150, sound="chirp")
                            last_sound = now
                        present = True
                    if period <= 1000 and record.get("frame_us", 0) <= 1000000:
                        target_x = clamp((box[0] / width * 2.0 - 1.0) * 0.85)
                        target_y = clamp((box[1] / height * 2.0 - 1.0) * 0.65)
                        if MIRROR_X:
                            target_x = -target_x
                    else:
                        target_x = target_y = 0.0
                else:
                    streak = 0
            if now - last_seen > LOST_MS:
                if present:
                    face.set("bored", transition_ms=300)
                present = False
                target_x = target_y = 0.0
            gaze_x += (target_x - gaze_x) * 0.2
            gaze_y += (target_y - gaze_y) * 0.2
            if abs(gaze_x - sent_x) + abs(gaze_y - sent_y) > 0.025:
                face.look(gaze_x, gaze_y)
                sent_x, sent_y = gaze_x, gaze_y
            status = pipeline.status(detector)
            if status["done"]:
                raise RuntimeError("Face detector stopped: " + str(status["last_error"]))
            time.sleep_ms(POLL_MS)
    except KeyboardInterrupt:
        pass
    finally:
        try:
            if detector is not None:
                pipeline.destroy(detector)
        finally:
            if face is not None:
                face.close()
            if previous != slot:
                restore = pipeline.start(SOURCE, "himax", previous, 1)
                pipeline.destroy(restore)


main(int(sys.argv[1]) if len(sys.argv) > 1 else 0)
