import numpy as np
import cv2
import cv2.aruco as aruco
import time



camera = cv2.VideoCapture(0) #is this needed?
#camera = PiCamera() #what's the equivalent for this?

font = cv2.FONT_HERSHEY_SIMPLEX


while True:
    return_value,img = camera.read()


    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
    parameters =  aruco.DetectorParameters_create()

    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
    #print(corners)

    id_list = list(ids)

    id_list2 = []
    for i in id_list:
        id_list2.append(i[0]) #where is this being used?


    image = aruco.drawDetectedMarkers(img, corners, ids)

        # get_head_and_shoulders()
        # get_stoop()

    cv2.putText(image,'RR2/CamTestv0.1.2706',(10,30),font,1,(255,0,0),1,cv2.LINE_AA)
    cv2.putText(image,fps(),(10,50),font,1,(255,0,255),1,cv2.LINE_AA)
    cv2.imshow('frame',image)

    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
#cap.truncate(0) # Important!

camera.close()
cv2.destroyAllWindows()