import time
import zmq
import socket
import threading
import queue

import numpy as np

from msgpack import loads
from matplotlib import pyplot as plt

SAMPLING_FREQUENCY = 120 # Hz
SECONDS_TO_RECORD = 3

data_queue = queue.Queue()

def request_pupil_time(socket):
    """
    from https://github.com/pupil-labs/pupil-helpers/blob/master/python/simple_realtime_time_sync.py
    """
    socket.send_string("t")
    pupil_time = socket.recv()
    return float(pupil_time)


def measure_clock_offset(socket, clock_function):
    """
    from https://github.com/pupil-labs/pupil-helpers/blob/master/python/simple_realtime_time_sync.py
    """
    local_time_before = clock_function()
    pupil_time = request_pupil_time(socket)
    local_time_after = clock_function()

    local_time = (local_time_before + local_time_after) / 2.0
    clock_offset = pupil_time - local_time
    return clock_offset

def measure_clock_offset_stable(socket, clock_function, nsamples=10):
    """
    from https://github.com/pupil-labs/pupil-helpers/blob/master/python/simple_realtime_time_sync.py
    """
    assert nsamples > 0, "Requires at least one sample"
    offsets = [measure_clock_offset(socket, clock_function) for x in range(nsamples)]
    return sum(offsets) / len(offsets)  # mean offset

class PupilGetter(threading.Thread):

    def __init__(self, data_queue, subscriber):
        super().__init__()
        self.queue = data_queue
        self.subscriber = subscriber
        self.running = True

    def run(self):
        _, payload = self.subscriber.recv_multipart()
        self.queue.put(payload)
        while self.running:
            _, payload = self.subscriber.recv_multipart()
            # if payload == b'' or payload == '' or payload == None:
            #     print('QUEUE EMPTY!!') # <----- THIS NEVER HAPPENS!!
            self.queue.put(payload)

    def stop(self):
        self.running = False


class PupilReader(threading.Thread):

    def __init__(self, data_queue):
        super().__init__()
        self.queue = data_queue
        self.running = True
        self.gaze_data = []
        self.timestamps = []

    def run(self):
        while self.running:
            frame = self.queue.get()
            frame_data = loads(frame, raw=False)
            self.gaze_data.append(frame_data)
            self.timestamps.append(frame_data['timestamp'])

    def stop(self):
        self.running = False

ADDRESS = "127.0.0.1"  # remote ip or localhost
REQUEST_PORT = "50020"  # same as in the pupil remote gui

context = zmq.Context()
pupil_remote = context.socket(zmq.REQ)
pupil_remote.connect(f"tcp://{ADDRESS}:{REQUEST_PORT}")

pupil_remote.send_string("SUB_PORT")
sub_port = pupil_remote.recv_string()
print(sub_port)

# open a sub port to listen to pupil
this_subscriber = context.socket(zmq.SUB)
this_subscriber.connect(f'tcp://{ADDRESS}:{sub_port}')
this_subscriber.subscribe('gaze.')  # receive all gaze messages

local_clock = time.perf_counter

offset = measure_clock_offset_stable(pupil_remote, clock_function=local_clock)
print(offset)


start_time = time.time()

pupil_getter = PupilGetter(data_queue, this_subscriber)
pupil_reader = PupilReader(data_queue)
pupil_getter.start()
pupil_reader.start()

time.sleep(10)

end_time = time.time()

pupil_getter.stop()
pupil_reader.stop()
pupil_getter.join()
pupil_reader.join()

time_elapsed = end_time - start_time
timestamps_elapsed = pupil_reader.timestamps[-1] - pupil_reader.timestamps[0]

print(time_elapsed)
print(timestamps_elapsed)

# Plot timestamps
plt.plot(pupil_reader.timestamps)
plt.ylabel('timestamp value')
plt.xlabel('sample')
plt.show()