# $Header$
#
# this code is from master_listener.py script from Microgate

import numpy as np
import socket
import struct
import time
import matplotlib.pyplot as plt
import threading

## Thread definition
class PLOT_THREAD(threading.Thread):
    def __init__(self):#, queue, out_queue):
        threading.Thread.__init__(self)
        # self.queue = queue
        # self.out_queue = out_queue
        self.newImg = False
        self.RUN = False
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        # binding itself on port 8000. not bcu ip address
        self.sock.bind(('192.168.5.112', 8000))
        self.sock.settimeout(2)

    def newImage(self, img, frame_cnt):
        if not self.newImg:
            self.img = img
            self.newImg = True
            self.frameCnt = frame_cnt

    def run(self):
        pck_inprogress = -1
        frame_len = -1
        prev_frame_cnt = -1
        old_frame_cnt = -1
        num_frames = 0
        restart = 0
        frame = []
        while True:
            if self.RUN:
                if restart==0:
                    print("waiting on port:", 8000)
                    restart=1
                try:
                    data = self.sock.recv(8000)
                except socket.timeout:
                    print('timeout')
                    continue
                pck_header = struct.unpack('<HLLHH', data[0:14])
                # detected new frame start
                if pck_header[2] == 0:
                    if frame_len != -1 and frame_len != 0:
                        pass
                        # print("invalid packet received:        -       , len(%5d/%5d)" % (frame_len, len(frame)))
                    frame_len = pck_header[1]
                    num_frames += 1
                frame.append(data[14:]);
                frame_len -= len(data[14:])
                # continue
                if frame_len == 0:
                    frame = b''.join(frame)
                    frame_cnt = struct.unpack('<L', frame[0:4])
                    frame_cnt = frame_cnt[0]
                    if len(frame) == 480 or len(frame) == 64:
                        if prev_frame_cnt != -1 and frame_cnt != prev_frame_cnt + 1:
                            pass
                            # print('Frame lost detected')
                        prev_frame_cnt = frame_cnt
                        if   old_frame_cnt == -1:
                            t0 = time.time()
                            old_frame_cnt = frame_cnt
                        else:
                            t1 = time.time()
                            if t1 - t0 >= 1:
                                print('Camera is running @ %6.1f Hz' % ((frame_cnt - old_frame_cnt)/(t1-t0)))
                                t0 = t1
                                old_frame_cnt = frame_cnt
                    elif len(frame) == 8200:
                        img = np.array(struct.unpack('<%iH' % int((len(frame)-8)/2), frame[4:-4]))
                        img_hdr = np.copy(img[0:4])
                        img[0:4] = np.mean(img[4:])
                        img = img.reshape((64,64))
                        if not self.newImg:
                            self.newImg = True
                            self.img=img.copy()
                            self.frameCnt=frame_cnt
                    elif len(frame) == 16136:
                        img = np.array(struct.unpack('<%iH' % int((len(frame)-8)/2), frame[4:-4]))
                        img_hdr = np.copy(img[0:4])
                        img[0:4] = np.mean(img[4:])
                        img = img.reshape((84,96))
                        if not self.newImg:
                            self.newImg = True
                            self.img=img.copy()
                            self.frameCnt=frame_cnt
                    elif len(frame) == 163848:
                        img = np.array(struct.unpack('<%iH' % int((len(frame)-8)/2), frame[4:-4]))
                        img_hdr = np.copy(img[0:4])
                        img[0:4] = np.mean(img[4:])
                        img = img.reshape((256,320))
                        if not self.newImg:
                            self.newImg = True
                            self.img=img.copy()
                            self.frameCnt=frame_cnt
                    elif len(frame) == 655368:
                        img = np.array(struct.unpack('<%iH' % int((len(frame)-8)/2), frame[4:-4]))
                        img_hdr = np.copy(img[0:4])
                        img[0:4] = np.mean(img[4:])
                        img = img.reshape((512,640))
                        if not self.newImg:
                            self.newImg = True
                            self.img=img.copy()
                            self.frameCnt=frame_cnt
                    frame = []

            else:
                pck_inprogress = -1
                frame_len = -1
                prev_frame_cnt = -1
                old_frame_cnt = -1
                num_frames = 0
                restart = 0
                frame = []

def run():
    ## Listener startup
    plt.ion()
    ptThread = PLOT_THREAD()
    ptThread.setDaemon(True)
    ptThread.start()

    ## Listener ON
    ptThread.RUN=True
    fig = []
    img_shape = (0, 0)
    while True:  # num_frames < 200:
        if ptThread.newImg:
            if fig == []:
                fig = plt.figure()
                im = plt.imshow(ptThread.img, vmin=0, vmax=8000)
                img_shape = ptThread.img.shape
            elif ptThread.img.shape != img_shape:
                plt.clf()
                im = plt.imshow(ptThread.img, vmin=0, vmax=8000)
                img_shape = ptThread.img.shape
            # print('Frame counter %d' % (ptThread.frameCnt))
            im.set_data(ptThread.img)
            plt.title('Frame counter %d' % (ptThread.frameCnt))
            ptThread.newImg = False
        else:
            plt.pause(0.001)

    ## Listener OFF
    ptThread.RUN=False
