import cv2, numpy as np
from calibration import object_calibration
from hand_detection import detect_hands, check_hand
from helper import compute_distance
from hud import find_hud_placement
import math

from config import PIXELS_PER_INCH, CIRCLE_TOUCH_THRESHOLD
from config import FRAME_INTERVAL, TARGET_FPS
from config import DEBUG
from setup import InitCameras
#from hud import draw_hud_box, draw_pdf_page
import time

# ----- Global State -----
measuring = False             # measurement in progress
start_pt = None               # measurement start point
calibrating = False           # pinch-based calibration flag
object_calibrating = False    # object calibration flag
fingertip_idx_global = None   # last detected fingertip position

hud_pos = None
hud_rot = None
register_hud = False

# ----- Per-camera Processing -----
def process_frame(frame, camera_id: int = 0):
    global measuring, start_pt, calibrating
    global object_calibrating
    global fingertip_idx_global
    
    global register_hud, hud_pos, hud_rot

    frame_vis = cv2.flip(frame, 1)

    # Hand detection
    results = detect_hands(frame_vis)

    fingertip_idx = None
    if results.multi_hand_landmarks:
        hand = results.multi_hand_landmarks[0]
        fingertip_idx, measuring, start_pt = check_hand(hand, frame_vis, object_calibrating)

    fingertip_idx_global = fingertip_idx
 
    # object calibration
    if object_calibrating and fingertip_idx is not None:
        object_calibration(frame_vis, fingertip_idx, CIRCLE_TOUCH_THRESHOLD)
        object_calibrating = False
    
    # place hud
    res = None
    if hud_pos is None:
        res = find_hud_placement(frame_vis)
    else:
        res = hud_pos, hud_rot

    # create proj output
    projection_out = np.zeros_like(frame)
    
    # measuring
    if measuring and start_pt and fingertip_idx_global:
        cv2.line(projection_out, start_pt, fingertip_idx_global, (255,0,0),2)
        px = compute_distance(start_pt, fingertip_idx_global)
        inch = px/PIXELS_PER_INCH; cm = inch*2.54
        mid = ((start_pt[0]+fingertip_idx_global[0])//2,(start_pt[1]+fingertip_idx_global[1])//2)
        cv2.putText(projection_out,f"{inch:.2f}in/{cm:.1f}cm",(mid[0]+10,mid[1]-10), cv2.FONT_HERSHEY_SIMPLEX,0.7,(255,0,255),2)

    # hud
    if res is not None or hud_pos:
        if hud_pos:
            arrow_tip = hud_pos
            dir_vec = hud_rot
        else:
            arrow_tip, dir_vec = res

        dx, dy = dir_vec
        arrow_angle = math.atan2(dy, dx)   # result in radians
        #quad = draw_hud_box(projection_out, arrow_tip, arrow_angle, size=(400,400))
        #draw_pdf_page(projection_out, page_index=0, dst_quad=quad)

        if register_hud:
            hud_pos = arrow_tip
            hud_rot = dir_vec
            register_hud = False

    
    mask = cv2.cvtColor(projection_out, cv2.COLOR_BGR2GRAY) > 0
    frame_vis[mask] = projection_out[mask]

    return frame_vis, projection_out

# ----- Main ----- -------------------------------------------------------------------------------------
def main():
    all_captures = InitCameras()
    
    print(f"Running at up to {TARGET_FPS} FPS (interval={FRAME_INTERVAL:.3f}s)")
    print("Press 'o' for circle calib, 'q' to quit.")

    last_time = time.time()
    while True:
        processed_frames = []

        for camera_id, capture in enumerate(all_captures):
            ok, frame = capture.read()
            if not ok or frame is None:
                continue

            projection, debug = process_frame(frame, camera_id)
            processed_frames.append((camera_id, projection, debug))

        if not processed_frames:
            break

        for camera_id, projection, debug in processed_frames:
            if DEBUG:
                cv2.imshow(f'Debug Output (cam {camera_id})', debug)
            cv2.imshow(f'Projector Output (cam {camera_id})', projection)

        key = cv2.waitKey(1) & 0xFF

        if key == ord('q'):
            break
        elif key == ord('o'):
            global object_calibrating
            object_calibrating = True
            print("Entered circle calibration mode.")
        elif key == ord('j'):
            global register_hud, hud_pos
            register_hud = True
            hud_pos = None

        # ——— frame‐rate limiting ———
        now     = time.time()
        elapsed = now - last_time
        to_wait = FRAME_INTERVAL - elapsed
        if to_wait > 0:
            time.sleep(to_wait)
        last_time = time.time()

    for capture in all_captures:
        capture.release()

    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()