import numpy as np
import imutils
import cv2 as cv
import urllib.request
import ssl


video=cv.VideoCapture(0)
address='http://192.168.1.6:8080/shot.jpg'
video.open(address)

Known_distance = 30  
Known_width = 5.7
whT = 320 #width,high,Target 320 cause we re using 320 version yolo
confThreshold =0.5
nmsThreshold= 0.2
Distance_level = 0

# Define the codec and create VideoWriter object
fourcc = cv.VideoWriter_fourcc(*'XVID')
out = cv.VideoWriter('output21.mp4', fourcc, 30.0, (640, 480))

#extract the file "coco caméra" qui contient la liste des objets à détectés
classesFile = "/home/pi/Desktop/coco.names"
classNames = []
with open(classesFile, 'rt') as f:
    classNames = f.read().rstrip('\n').split('\n')
print(classNames)

#importer la configuration des fichier .cfg et .weights
modelConfiguration = "/home/pi/Desktop/yolov3-tiny.cfg"
modelWeights = "/home/pi/Desktop/yolov3-tiny.weights"
#creating our network, we re going to read from darknet model config nd model weights
net = cv.dnn.readNetFromDarknet(modelConfiguration, modelWeights)
net.setPreferableBackend(cv.dnn.DNN_BACKEND_OPENCV)
net.setPreferableTarget(cv.dnn.DNN_TARGET_CPU)


def FocalLength(measured_distance, real_width, width_in_rf_image):
    focal_length = (width_in_rf_image * measured_distance) / real_width
    return focal_length


def Distance_finder(Focal_Length, real_object_width, object_width_in_frame):
    distance = (real_object_width * Focal_Length)/object_width_in_frame
    return distance


def findObjects(outputs,img):
    
    hT, wT, cT = img.shape
    bbox = []
    classIds = []
    confs = []
    for output in outputs:
        for det in output:
            scores = det[5:]
            classId = np.argmax(scores)
            confidence = scores[classId]
            
            if confidence > confThreshold:
                w,h = int(det[2]*wT) , int(det[3]*hT)
                x,y = int((det[0]*wT)-w/2) , int((det[1]*hT)-h/2)
                bbox.append([x,y,w,h])
                classIds.append(classId)
                confs.append(float(confidence))
     #déterminer l'indice du box dont la pourcentage du conf est la plus grande
    indices = cv.dnn.NMSBoxes(bbox, confs, confThreshold, nmsThreshold)
    object_width = 0
    object_x, object_y = 0,0
    object_center_x = 0
    object_center_y = 0
    
    for i in indices:
        i = i[0]
        box = bbox[i]
        x, y, w, h = box[0], box[1], box[2], box[3]
            #draw the box
        cv.rectangle(img, (x, y), (x+w,y+h), (255, 0 , 255), 2)
        cv.putText(img, f'{classNames[classIds[i]].upper()}',(x,y-10),cv.FONT_HERSHEY_SIMPLEX,0.6,(255,0,255),2)
        print(classNames[classIds[i]])
        object_width = w
        object_center =[]
        object_center_x = int(w/2)+x
        object_center_y = int(h/2)+y
        
        object_x = x
        object_y = y
        
    return object_width, bbox, object_center_x, object_center_y
    

# reading reference image from directory
ref_image = cv.imread("Image_test.PNG")
blob = cv.dnn.blobFromImage(ref_image, 1 / 255, (whT, whT), [0, 0, 0], 1, crop=False)
net.setInput(blob)
layersNames = net.getLayerNames()
outputNames = [(layersNames[i[0] - 1]) for i in net.getUnconnectedOutLayers()]

outputs = net.forward(outputNames)
ref_image_object_width, _, _, _ = findObjects(outputs,ref_image)

Focal_length_found = FocalLength( Known_distance, Known_width, ref_image_object_width)
print(Focal_length_found)
cv.imshow("ref_image", ref_image)

   

while True:
    imgResp = urllib.request.urlopen(address)
    imgNp = np.array(bytearray(imgResp.read()), dtype=np.uint8)
    img = cv.imdecode(imgNp, -1)
    #convertir l'image de la caméra into our network, network accepts only a blob file(binary large object)
    blob = cv.dnn.blobFromImage(img, 1 / 255, (whT, whT), [0, 0, 0], 1, crop=False)
    net.setInput(blob)
    layersNames = net.getLayerNames()

    #print(layerNames)
    #extract only the output layers
    outputNames = [(layersNames[i[0] - 1]) for i in net.getUnconnectedOutLayers()]
    outputs = net.forward(outputNames)
    findObjects(outputs,img)
    object_width_in_frame, bbox, OB_X ,OB_Y = findObjects(outputs,img)

    for (object_x, object_y, object_w, object_h) in bbox:
        Distance=Distance_finder(Focal_length_found,Known_width,object_width_in_frame )
        Distance = round(Distance, 2)
        cv.putText(img, f"Distance {Distance} Inches",(object_x-6, object_y-6), cv.FONT_HERSHEY_COMPLEX, 0.5, (0,0,0), 2)
        print(Distance*0.0254)
    cv.imshow('temp',cv.resize(img,(600,400)))
    
    q = cv.waitKey(1)
    if q == ord("q"):
        break
         
video.release()
cv.destroyAllWindows