# -*- coding: utf-8 -*-

import sys
import socket
import struct
import time
import numpy as np
import threading
import datetime as dt

plot_thread = None

def switch_image_size(frame_len):
    if frame_len == 2056:
        return (32, 32)
    elif frame_len == 8200:
        return (64, 64)
    elif frame_len == 16136:
        return (96, 84)
    elif frame_len == 129032:
        return (288, 224)
    elif frame_len == 131080:
        return (256, 256)
    elif frame_len == 163848:
        return (320, 256)
    elif frame_len == 524296:
        return (512, 512)
    elif frame_len == 655368:
        return (512, 640)
    else:
        pass #throw;

## Thread definition
class PLOT_THREAD(threading.Thread):
    def __init__(self):#, queue, out_queue):
        threading.Thread.__init__(self)

        self.RUN = False
        self.CLOSE = False
        self.port = 8001
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)

        try:
#            IP = "127.0.0.1"
#            try:
#                IP = socket.gethostbyname(socket.gethostname())
#            except Exception as e:
#                IP = "127.0.0.1"
            s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            try:
                s.connect(('192.168.61.253',1))
                IP = s.getsockname()[0]
            except:
                IP = '127.0.0.1'
            finally:
                s.close()
            self.sock.bind((IP, self.port)) #IP of sharknirws
            # self.sock.bind(('192.168.61.113', self.port)) #IP of sharknirws
        except socket.error as e:
            print("Socket Error")
            print(e)
            self.sock.shutdown(1)
            self.sock.close()
            sys.exit(1)
        except Exception as e:
            print("Generic exception")
            print(e)
            self.sock.shutdown(1)
            self.sock.close()
            sys.exit(1)

        self.sock.settimeout(2)
        self.lock = threading.Lock()
        self.centroid_x = []
        self.centroid_y = []
        self.ci_x = 0
        self.ci_y = 0
        self.centroid_items = 0
        self.shape = np.array([0 for i in range(96)])
        self.cloud_samples = 2000

    def run(self):

        frame_len = -1
        prev_frame_cnt = -1
        old_frame_cnt = -1
        first_frame_cnt = 0;
        num_frames = 0
        restart = 0
        img_frame = []
        timeout_counter = 0
        command_string = '<' + 'f'*97
        tt_string = '<fLffLLff' # power, skipped, centroid x y , 2 not used, tip, tilt
        diagnostic_frame_counter = 0

        while True:
            try :
                if self.RUN:
                    if restart==0:
                        #print("waiting on port:", self.port)
                        restart=1
                    try:
                        data = self.sock.recv(8000)
                    except socket.timeout:
                        timeout_counter += 1
                        if timeout_counter % 5 == 0:
                            pass
                        if self.CLOSE:
                            break
                        else:
                            continue
                    except Exception as e:
                        print("Exception : ", e)
                        self.CLOSE = True

                    ## Network Packet Received
                    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

                        frame_len = pck_header[1]
                        num_frames += 1

                    img_frame.append(data[14:])
                    #print(len(img_frame))
                    frame_len -= len(data[14:])

                    if frame_len == 0:

                        ## Diagnostic Packet Complete
                        img_frame = b''.join(img_frame)
                        img_frame_len = len(img_frame)
                        frame_cnt = struct.unpack('<L', img_frame[0:4])
                        frame_cnt = frame_cnt[0]

                        ## Diagnostic Frame
                        if img_frame_len == 480 or img_frame_len == 64:
                            #print("diag frame")

                            prev_frame_cnt = frame_cnt

                            diagnostic_frame_counter += 1
                            if old_frame_cnt == -1:
                                t0 = time.time()
                                old_frame_cnt = frame_cnt
                                first_frame_cnt = frame_cnt
                                print("first_frame_cnt = : %d" % first_frame_cnt)
                            else:
                                t1 = time.time()
                                if t1 - t0 >= 60:
                                    t0 = t1
                                    old_frame_cnt = frame_cnt
                                    
                            #print( len(img_frame))

                            DM_command = np.array(struct.unpack(command_string, img_frame[16:16+97*4]))
                            TT_command = struct.unpack(tt_string, img_frame[16 + 104*4:16+104*4 + 32])
                            #print(len(DM_command), len(img_frame))

                            ####################
                            ## Centroid Cloud ##
                            ####################

                            self.lock.acquire()
                            self.centroid_x.append(TT_command[2])
                            self.centroid_y.append(TT_command[3])
                            self.centroid_items += 1

                            if self.centroid_items > self.cloud_samples:
                                if len(self.centroid_x) > self.cloud_samples:
                                    self.centroid_x = self.centroid_x[-int(self.cloud_samples):]
                                    self.centroid_y = self.centroid_y[-int(self.cloud_samples):]
                            self.lock.release()

                            self.cx = np.mean(self.centroid_x)
                            self.cy = np.mean(self.centroid_y)

                            self.centroid_matrix, self.yedges, self.xedges = np.histogram2d(self.centroid_y, self.centroid_x, bins=21)

                            #######################
                            ## Centroid Integral ##
                            #######################

                            if self.ci_x == 0 and self.ci_y == 0:
                                self.ci_x = round(TT_command[2]*32)
                                self.ci_y = round(TT_command[3]*32)

                            ##############
                            ## DM Shape ##
                            ##############

                            self.shape = DM_command #* np.array(signs)
                            self.tt_cmd = TT_command

                        ## Pixel Frame
                        else:

                            #print("pixel frame")
                            ################
                            ## CRED image ##
                            ################

                            self.img = np.array(struct.unpack('<%iH' % int((img_frame_len-8)/2), img_frame[4:-4]))
                            self.img = self.img.reshape(switch_image_size(img_frame_len))
                            #print("master listener shape:"+str(self.img.shape))
                            self.frameCnt = frame_cnt
                            self.img_frame_len = img_frame_len


                        img_frame = []

                else:
                    frame_len = -1
                    prev_frame_cnt = -1
                    old_frame_cnt = -1
                    num_frames = 0
                    restart = 0
                    img_frame = []
                    time.sleep(0.05)

                    if self.CLOSE:
                        break
            except KeyboardInterrupt:
                print(" Intercepted Keyboard Interrupt")
                break

        print("Exiting PLOT_THREAD.run()")
