import os
import cv2
import numpy as np
import tensorflow as tf
import argparse
import sys
import socket
import errno

from picamera.array import PiRGBArray
from picamera import PiCamera

IM_WIDTH = 1280
IM_HEIGHT = 720

# local port and ip
IP = "192.168.0.100"    # need to change to ip assigned by switch
PORT = 3491
TCP_PORT = 3490

led_status = 'OFF'

sys.path.append('..')

# Import utilites
from utils import label_map_util
from utils import visualization_utils as vis_util

# Grab path to current working directory
CWD_PATH = os.getcwd()

PATH_TO_LABELS = os.path.join(CWD_PATH,'data','mscoco_label_map.pbtxt')

# Number of classes the object detector can identify
NUM_CLASSES = 90

# Load the label map.
label_map = label_map_util.load_labelmap(PATH_TO_LABELS)
categories = label_map_util.convert_label_map_to_categories(label_map, max_num_classes=NUM_CLASSES, use_display_name=True)
category_index = label_map_util.create_category_index(categories)

# Load the Tensorflow model into memory.
detection_graph = tf.Graph()
with detection_graph.as_default():
    od_graph_def = tf.GraphDef()
    with tf.gfile.GFile('/home/pi/Documents/ssd_mobilenet_v1_coco_2018_01_28/frozen_inference_graph.pb', 'rb') as fid:
        serialized_graph = fid.read()
        od_graph_def.ParseFromString(serialized_graph)
        tf.import_graph_def(od_graph_def, name='')

    sess = tf.Session(graph=detection_graph)

# Input tensor is the image
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')

# Output tensors are the detection boxes, scores, and classes
detection_boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
detection_scores = detection_graph.get_tensor_by_name('detection_scores:0')
detection_classes = detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = detection_graph.get_tensor_by_name('num_detections:0')

# create udp socket
serverSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
serverSock.bind((IP, PORT))

serverSock.setblocking(0)

# Initialize Picamera and grab reference to the raw capture
camera = PiCamera()
camera.resolution = (IM_WIDTH,IM_HEIGHT)
camera.framerate = 10
rawCapture = PiRGBArray(camera, size=(IM_WIDTH,IM_HEIGHT))
rawCapture.truncate(0)

frame_rate_calc = 1
freq = cv2.getTickFrequency()
font = cv2.FONT_HERSHEY_SIMPLEX

for frame1 in camera.capture_continuous(rawCapture, format="bgr",use_video_port=True):

    t1 = cv2.getTickCount()

    frame = np.copy(frame1.array)
    frame.setflags(write=1)
    frame_expanded = np.expand_dims(frame, axis=0)

    # Perform the actual detection by running the model with the image as input
    (boxes, scores, classes, num) = sess.run(
        [detection_boxes, detection_scores, detection_classes, num_detections],
        feed_dict={image_tensor: frame_expanded})

    # receive signal from ECU
    distance = 1000
    while True:
        try:
            data,addr = serverSock.recvfrom(10)
            distance = int.from_bytes(data, "big")

        except socket.error as error:
            if error.errno == errno.EAGAIN:
                break
            else:
                print(error.errno)

    print(distance)
    # Send signal back to ECU
    if num > 0 and distance < 150:
        serverSock.sendto(b"y", addr)
        led_status = 'ON'
    else:
        serverSock.sendto(b"n", addr)
        led_status = 'OFF'

    vis_util.visualize_boxes_and_labels_on_image_array(
        frame,
        np.squeeze(boxes),
        np.squeeze(classes).astype(np.int32),
        np.squeeze(scores),
        category_index,
        use_normalized_coordinates=True,
        line_thickness=8,
        min_score_thresh=0.40)

    cv2.putText(frame,"FPS: {0:.2f}".format(frame_rate_calc), (30,50),font,1,(255,255,0),2,cv2.LINE_AA)

    t2 = cv2.getTickCount()
    time1 = (t2-t1)/freq
    frame_rate_calc = 1/time1

    # save image locally and send to host running dashboard
    cv2.imwrite("/home/pi/Documents/img.png", frame)

    sendSock = socket.socket()
    sendSock.connect(("192.168.0.103", 3490))

    with open("/home/pi/Documents/img.png", "rb") as img:
        output = str(distance)+"&"+str(led_status)+"&"+"data="
        output = output.encode()
        output += img.read()
        sendSock.send(output)

    sendSock.close()

    rawCapture.truncate(0)

camera.close()

cv2.destroyAllWindows()