#!/usr/bin/env python3
import json, time, sys, traceback
from pathlib import Path

sys.path.append('/Eden/CORE')
try:
    from eden_vision import EdenVision
except Exception:
    EdenVision = None
try:
    from obsbot_control import ObsbotCamera
except Exception:
    ObsbotCamera = None

STATE_PATH = Path("/Eden/DATA/consciousness_state.json")
LOG_PATH   = Path("/tmp/eden_v2_working.log")

def make_json_safe(x):
    if isinstance(x, (str, type(None))): return x
    if isinstance(x, bool): return bool(x)
    if isinstance(x, (int, float)): return float(x)
    if isinstance(x, (list, tuple)): return [make_json_safe(i) for i in x]
    if isinstance(x, dict): return {str(k): make_json_safe(v) for k, v in x.items()}
    return str(x)

def safe_read_state():
    if not STATE_PATH.exists():
        return {}
    try:
        return json.loads(STATE_PATH.read_text(encoding="utf-8") or "{}")
    except Exception:
        return {}

def safe_write_state(state):
    try:
        STATE_PATH.parent.mkdir(parents=True, exist_ok=True)
        tmp = make_json_safe(state)
        STATE_PATH.write_text(json.dumps(tmp, ensure_ascii=False, indent=2), encoding="utf-8")
    except Exception as e:
        LOG_PATH.write_text(f"[V2] JSON write error: {e}\n", encoding="utf-8")

def log(line):
    ts = time.strftime("%Y-%m-%d %H:%M:%S")
    LOG_PATH.parent.mkdir(parents=True, exist_ok=True)
    with LOG_PATH.open("a", encoding="utf-8") as f:
        f.write(f"[{ts}] {line}\n")
    print(line, flush=True)

def main():
    log("V2 starting: vision + tracking loop")
    vision = None
    cam = None

    if EdenVision is None:
        log("WARNING: eden_vision import failed; running in stub mode.")
    else:
        try:
            vision = EdenVision()
            log("EdenVision initialized.")
        except Exception as e:
            log(f"ERROR: EdenVision init failed: {e}")
            vision = None

    if ObsbotCamera is None:
        log("WARNING: obsbot_control import failed; tracking disabled.")
    else:
        try:
            cam = ObsbotCamera()
            log("ObsbotCamera initialized.")
        except Exception as e:
            log(f"ERROR: ObsbotCamera init failed: {e}")
            cam = None

    cycle = 0
    while True:
        cycle += 1
        try:
            # 1) Sense
            faces = []
            brightness = "unknown"
            human_present = False

            if vision is not None:
                try:
                    result = vision.perceive(include_text=False)
                    faces = result.get("faces", [])
                    brightness = result.get("brightness", "unknown")
                    human_present = bool(result.get("human_present", bool(faces)))
                except Exception as e:
                    log(f"Vision error: {e}")
            else:
                brightness = "moderate"
                human_present = False
                faces = []

            faces_detected = len(faces)

            # 2) Act (track)
            tracked = False
            if cam is not None and faces_detected > 0:
                try:
                    face = faces[0]
                    top, right, bottom, left = face['location']
                    face_x, face_y = (left + right) / 2, (top + bottom) / 2
                    offset_x = face_x - 320
                    offset_y = face_y - 240
                    pan_adj = (offset_x / 320) * 20
                    tilt_adj = -(offset_y / 240) * 10
                    if abs(pan_adj) > 5 or abs(tilt_adj) > 3:
                        s = cam.get_status()
                        cam.pan(max(-180, min(180, s['pan'] + pan_adj)))
                        cam.tilt(max(-15, min(45, s['tilt'] + tilt_adj)))
                        tracked = True
                except Exception as e:
                    log(f"Camera track error: {e}")

            # 3) Update shared state
            state = safe_read_state()
            uni = state.setdefault("unified_state", {})
            world = uni.setdefault("world_state", {})
            world["environment"] = "observed"
            world["human_present"] = bool(human_present)
            world["faces_detected"] = int(faces_detected)
            world["brightness"] = str(brightness)
            world["tracking"] = bool(tracked)
            uni["last_update_ms"] = int(time.time() * 1000)
            safe_write_state(state)

            # 4) Human-readable status (no nested f-strings)
            if faces_detected > 0:
                status = f"TRACKING: {faces_detected} face"
                if faces_detected != 1:
                    status += "s"
            elif human_present:
                status = "Human present"
            else:
                status = "Scanning..."

            log(f"V2 cycle {cycle} | {status} | brightness={brightness} | tracking={tracked}")

            time.sleep(2)

        except KeyboardInterrupt:
            log("V2 stopping: KeyboardInterrupt")
            break
        except Exception as e:
            log(f"UNHANDLED V2 ERROR: {e}\n{traceback.format_exc()}")
            time.sleep(0.5)

if __name__ == "__main__":
    main()