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

from config import CAMERA_INDICES, FRAME_WIDTH, FRAME_HEIGHT, PIXELS_PER_INCH, CIRCLE_TOUCH_THRESHOLD
from config import DEBUG

from hud import draw_hud_box, draw_pdf_page


# ----- 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):
    global measuring, start_pt, calibrating
    global object_calibrating
    global fingertip_idx_global
    
    global register_hud, hud_pos, hud_rot

    frame_out = cv2.flip(frame, 1)

    frame_vis = frame_out.copy()
    projection_debug = frame_out.copy()

    # Hand detection

    results = detect_hands(frame_out)

    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
    

    res = None
    if hud_pos is None:
        res = find_hud_placement(frame_vis)
    else:
        res = hud_pos, hud_rot

    if res is not None:
        arrow_tip, dir_vec = res
        draw_hud(frame_vis, arrow_tip, dir_vec)

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

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

    return frame_vis

# ----- Main ----- -------------------------------------------------------------------------------------
def main():
    global register_hud, hud_pos, hud_rot

    all_captures = []
    for idx in CAMERA_INDICES:
        capture = cv2.VideoCapture(idx)
        capture.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
        capture.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
        all_captures.append(capture)
    if not all(capture.isOpened() for capture in all_captures):
        print("Error: could not open all cameras")
        return

    print("Press 'o' for circle calib, 'q' to quit.")
    while True:
        frames = [capture.read()[1] for capture in all_captures]
        frame = next((f for f in frames if f is not None), None)

        if frame is None:
            break

        # process
        full_view = process_frame(frame)

        # create proj output
        projection_out = np.zeros_like(full_view)
        
        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)

        if hud_pos:
            dx, dy = hud_rot
            arrow_angle = math.atan2(dy, dx)   # result in radians
            quad = draw_hud_box(projection_out, hud_pos, arrow_angle, size=(400,400))
            draw_pdf_page(projection_out, page_index=0, dst_quad=quad)
            
        # overlay proj onto debug
        debug = full_view.copy()

        # overlay measurement
        if measuring and start_pt and fingertip_idx_global:
            cv2.line(debug, start_pt, fingertip_idx_global, (255,255,0),2)
            cv2.putText(debug,f"{inch:.2f}in/{cm:.1f}cm",(mid[0]+10,mid[1]-10),
            cv2.FONT_HERSHEY_SIMPLEX,0.7,(255,0,0),2)

        # show windows
        cv2.imshow('Debug Output', debug)
        cv2.imshow('Projector Output', projection_out)

        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'):
            register_hud = True
            hud_pos = None

    for capture in all_captures:
        capture.release()

    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()