#!/usr/bin/python
import detect
import timeit
import cv2
import socket
import time

from imutils.video import FPS
calibration_width = 0.75
image = "img7.jpg"
images = ("img.jpg", "img3.jpg", "img4.jpg", "img5.jpg", "img6.jpg", "img7.jpg", "img8.jpg")
#images = ("img.jpg", "img2.jpg")
video = True
def go():
    for file in images:
        items,output = detect.detect(calibration_width, file, True, False)
        print(str(items))
        if "Penny" in items:
            items.remove("Penny")
            itema = items[0]
            valid = True
            for item in items:
                if item != itema:
                    print("Too many items!")
                    valid = False
                    break
            if valid:
                print("Found " + itema)
                #selectedBin = sort.sort(itema)
                #control_motor.goToBin(selectedBin)

def sendString(string):
    print("[ INFO ] Sending message: " + string)
    try:
        conn.sendall(string.encode('utf-8'))
    except socket.error:
        print("[ EXIT ] Socket connection error.")


if not video:
	elapsed_time = timeit.timeit(go, number=1)/1
	print(elapsed_time)

else :
    #capture = cv2.VideoCapture('tcpclientsrc port=5000 host=192.168.1.129 ! gdpdepay !  rtph264depay ! avdec_h264 ! videoconvert ! videorate ! video/x-raw,framerate=5/1 ! appsink', cv2.CAP_GSTREAMER)
    #capture = cv2.VideoCapture('tcpclientsrc host=192.168.1.129 port=5000 ! gdpdepay ! rtph264depay ! avdec_h264 ! autovideosink sync=false', cv2.CAP_GSTREAMER)
    capture = cv2.VideoCapture('udpsrc port=9000 caps="application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264" ! rtph264depay ! avdec_h264 ! videoconvert ! appsink sync=false', cv2.CAP_GSTREAMER)
    host = ''        # Symbolic name meaning all available interfaces
    port = 9001     # Arbitrary non-privileged port
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    #s.setsockopt(s, socket.SOL_SOCKET, socket.SO_REUSEADDR)
    s.bind((host, port))
    print("[ INFO ] TCP stream initialized. Waiting for client...")
    s.listen(1)
    conn, addr = s.accept()
    print('[ INFO ] Connected to ', addr)
    time.sleep(2)
    waitForPi = True
    while waitForPi is True:
        try:
            print("[ INFO ] Waiting for confirmation message.")
            data = conn.recv(1024)
            if not data: break
            encoding = 'utf-8'
            print ("[ INFO ] Message recieved: "+str(data))
            waitForPi = False
        except socket.error:
            print("[ EXIT ] Socket connection error.")
            break
    # server command for imx135 camera ./video2stdout | gst-launch-1.0 -v fdsrc ! h264parse !  rtph264pay config-interval=1 pt=96 ! gdppay ! tcpserversink host=192.168.43.152 port=5001
    # server command for udp: ./video2stdout | gst-launch-1.0 -v fdsrc !  h264parse ! rtph264pay config-interval=10 pt=96 ! udpsink host=192.168.43.40 port=9000
    x = 0
    currentItem = ""
    samples = 0
    intake = False
    while True:
        ret = capture.grab()
        x+=1
        if not ret:
            print('[ EXIT ] Empty frame received.')
            break
        #print('frame')
        if x > 1:
            ret,frame = capture.retrieve()
            frame = frame[300:1080, 300:1620]
            #frame = frame[170:900, 475:1100]
            items,output = detect.detect(calibration_width, frame, True, True)
            cv2.imshow('Item Sorter', output)
            x = 0
            if len(items) > 0:
                #items.remove("Penny")
                if intake is True:
                    sendString("Pause ")
                    intake = False
                itema = items[0]
                valid = True
                for item in items:
                    if item != itema:
                        #print("[ INFO ] More than one object present.")
                        valid = False
                        break
                if valid:
                    if currentItem == itema:
                        samples += 2
                    else:
                        currentItem = itema
                        samples = 0
                    if samples >= 10:
                        samples = 0
                        capture.release()
                        time.sleep(2)
                        sendString(currentItem)
                        waitForPi = True
                        while waitForPi is True:
                            try:
                                data = conn.recv(1024)
                                if not data: break
                                #print(str(data))
                                if str(data) == "b'Continue'":
                                    waitForPi = False
                                    capture = cv2.VideoCapture('udpsrc port=9000 caps="application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264" ! rtph264depay ! avdec_h264 ! videoconvert ! appsink sync=false', cv2.CAP_GSTREAMER)
                                    print("[ INFO ] Waiting for the next item")
                            except socket.error:
                                print("[ EXIT ] Socket connection error.")
                                break
            else:
                if samples > 0:
                    samples -= 1
                if samples == 0 and intake is False:
                    sendString("Intake")
                    intake = True
                    time.sleep(0.25)
        if cv2.waitKey(1)&0xFF == ord('q'):
            print("[ EXIT ] Shutting down.")
            s.shutdown(1)
            s.close()
            break