import cv2
import sys
import tensorflow as tf

import os
os.environ["PYSDL2_DLL_PATH"] = "C:\\Users\\vbob\\Downloads\\PySDL2-0.9.5\\sdl2"

import sdl2.ext 
          #  0           1            2            3              4            5           6             7          8          
videos = ['49KMH_0_3', '49KMH_1_2', '56KMH_0_3', '53KMH_0_3', '50KMH_0_2', '52KMH_0_3', '45KMH_0_3', '45KMH_1_2', '22KMH_0_1']
speeds = [49, 49, 56, 53, 50, 52, 45, 45, 22]
trackers = ['MedianFlow', 'TLD']

# Open the video
video_number =8
video_name = videos[video_number]
speed = speeds[video_number]
tracker = trackers[0]

showVideo = True

video = cv2.VideoCapture("videos/"+ video_name +".mp4")
frameTime = 10

offset = 55

nnmodel = './mobilenet_v1_41123.pb'
#nnmodel = './resnet_v2_283776.pb'

# Exit if video not opened.
if not video.isOpened():
    print("Could not open video")
    sys.exit()

# Read first frame.
ok, frame = video.read()
frame = cv2.resize(frame, (1280,720))
if not ok:
    print('Cannot read video file')
    sys.exit()

# Average FPS
totalFPS = 0
totalFrames = 0
timer = cv2.getTickCount()
timer2 = cv2.getTickCount()

# Tracker
if tracker == 'MedianFlow':
    tracker = cv2.TrackerMedianFlow_create()
if tracker == 'TLD':
    tracker = cv2.TrackerTLD_create()

l1Start = (81,31)
l1End = (1240,31)
l2Start = (81,131)
l2End = (1240,131)
distance = 4

# Frame Count
numberFrames = 0;
touchedGreen = 0;
touchedRed = 0;
frameArray = []
failedFps = 0


# Read the graph.
with tf.gfile.FastGFile(nnmodel, 'rb') as f:
    graph_def = tf.GraphDef()
    graph_def.ParseFromString(f.read()) 

with tf.Session() as sess:
    with tf.device("/cpu:0"):
        # Restore session
        sess.graph.as_default()
        tf.import_graph_def(graph_def, name='')
        ok, frame = video.read()
        frame = cv2.resize(frame, (1280,720))
        img =frame
        rows = img.shape[0]
        cols = img.shape[1]
        inp = cv2.resize(img, (300, 300))
        inp = inp[:, :, [2, 1, 0]]  # BGR2RGB

        # Run the model
        out = sess.run([sess.graph.get_tensor_by_name('num_detections:0'),
                        sess.graph.get_tensor_by_name('detection_scores:0'),
                        sess.graph.get_tensor_by_name('detection_boxes:0'),
                        sess.graph.get_tensor_by_name('detection_classes:0')],
                       feed_dict={'image_tensor:0': inp.reshape(1, inp.shape[0], inp.shape[1], 3)})

        bbox = [float(v) for v in out[2][0][0]]
        x = bbox[1] * cols
        y = bbox[0] * rows
        right = bbox[3] * cols
        bottom = bbox[2] * rows


        bbox = ((x+right)/2 - offset, (y+bottom)/2-offset, offset*2, offset*2)
        #bbox = cv2.selectROI(frame, False)
        ok = tracker.init(frame, bbox)
        if (bbox):

            while (video.isOpened()):
                # Read Current Frame
                ok, frame = video.read()
                if (not ok):
                    break
                frame = cv2.resize(frame, (1280,720))
                

                # Add lines
                cv2.line(frame,l1Start,l1End,(0,255,0),2)
                cv2.line(frame,l2Start,l2End,(0,0,255),2)

                # Update Tracker
                ok, bbox = tracker.update(frame)

                # If Tracker Succeeded
                if ok:
                    # Tracking success
                    p1 = (int(bbox[0]), int(bbox[1]))
                    p2 = (int(bbox[0] + bbox[2]), int(bbox[1] + bbox[3]))
                    frameArray.append((int((p1[0]+p2[0])/2),int((p1[1]+p2[1])/2)))
                    #print("p1 ", p1)
                    #print("p2 ", p2)
                    #print("l1Start ", l1Start)
                    #print("l1End ", l1End)
                    
                    if not sdl2.ext.liangbarsky(p1[0],p1[1],p2[0],p2[1],l1Start[0],l1Start[1],l1End[0],l1End[1])[0] == None:
                        #print(sdl2.ext.liangbarsky(p1[0],p1[1],p2[0],p2[1],l1Start[0],l1Start[1],l1End[0],l1End[1]))
                        cv2.rectangle(frame, p1, p2, (0,255,0), 2, -1)
                        for row in frameArray:
                            cv2.circle(frame, row, 1, (0,255,0), 2, -1)
                        touchedGreen = 1;
                    elif not sdl2.ext.liangbarsky(p1[0],p1[1],p2[0],p2[1],l2Start[0],l2Start[1],l2End[0],l2End[1])[0] == None:
                        cv2.rectangle(frame, p1, p2, (0,0,255), 2, -1)
                        for row in frameArray:
                            cv2.circle(frame, row, 1, (0,0,255), 2, -1)
                        touchedRed = 1;
                    else: 
                        for row in frameArray:
                            cv2.circle(frame, row, 1, (255,0,0), 2, -1)
                        cv2.rectangle(frame, p1, p2, (255,0,0), 2, -1)
                else :
                    # Tracking failure
                    #cv2.putText(frame, "Tracking failure detected", (100,80), cv2.FONT_HERSHEY_SIMPLEX, 0.75,(0,0,255),2)
                    failedFps += 1

                # If touched green, count frames
                if (touchedGreen == 1 and touchedRed == 0):
                    numberFrames += 1
                elif (touchedRed == 1 and touchedGreen == 0):
                    numberFrames += 1
                elif (touchedRed == 1 and touchedGreen == 1):
                    print("Speed: " + str(int((distance/(numberFrames/30))*3.6)))
                    print("Error: " + str(int((distance/(numberFrames/30))*3.6)-speed))
                    print("Percent Error: " + str((((distance/(numberFrames/30))*3.6)-speed)/speed))
                    cv2.putText(frame, "Estimated Speed: " + str(int((4/(numberFrames/30))*3.6)) + " Kph", (100,80), cv2.FONT_HERSHEY_SIMPLEX, 0.75,(0,0,255),2)

                # Display result
                if (showVideo):
                    cv2.imshow("DrawLine", frame)

                # Get FPS
                totalFPS += cv2.getTickFrequency() / (cv2.getTickCount() - timer)
                totalFrames += 1
                timer = cv2.getTickCount()
                
                # Close if ESC    
                k = cv2.waitKey(frameTime) & 0xff
                if k == 27: 
                    break
            
            else:
                print("No Car Detected")

    # Show FPS
    print("Average FPS: " + str(int(totalFPS/totalFrames)) + " FPS")
    print("Failed FPS" + str(failedFps))
    print("TotalTime" + str(cv2.getTickFrequency()/(cv2.getTickCount()-timer2)))
    sys.exit()

def rectangleIntersectedByLine(rectTopLeft, rectBotRight, lineStart, lineEnd):
    minX = lineStart[0]
    maxX = lineEnd[0]

    if (lineStart[0] > lineEnd[0]):
        minX = lineEnd[0]
        maxX = lineStart[0]
    
    if (maxX > rectBotRight[0]):
        maxX = rectBotRight[0]

    if (minX < rectTopLeft[0]):
        minX = rectTopLeft[0]

    if (minX > maxX):
        return False

    minY = lineStart[1]
    maxY = lineEnd[1]

    dx = lineEnd[0] - lineStart[0]

    if (abs(dx) > 0.000001):
        a = (lineEnd[1] - lineStart[1])/dx
        b = lineStart[1] - a * lineStart[0]
        minY = a * minX + b
        maxY = a * maxX + b

    if (minY > maxY):
        tmp = maxY
        maxY = minY
        minY = tmp

    if (maxY > rectBotRight[1]):
        maxY = rectBotRight[1]

    if (minY < rectTopLeft[0]):
        maxY = rectTopLeft[0]

    if (minY > maxY):
        return False 

    return True

def intersection(a,b):
  x = max(a[0], b[0])
  y = max(a[1], b[1])
  w = min(a[0]+a[2], b[0]+b[2]) - x
  h = min(a[1]+a[3], b[1]+b[3]) - y
  if w<0 or h<0: return () # or (0,0,0,0) ?
  return (x, y, w, h)

