import time
import numpy as np
from Queue import Queue
from PD__Python_ToF.libMesaSR import SrCam
from threading import Thread, Event
import csv
from copy import copy
from numpy import savez


def __get_coord_flt(cam):
    x = np.empty((cam.r, cam.c), dtype=np.float32)
    y = np.empty((cam.r, cam.c), dtype=np.float32)
    z = np.empty((cam.r, cam.c), dtype=np.float32)
    res=SrCam.lib.SR_CoordTrfFlt(cam.handle,x.ctypes,y.ctypes,z.ctypes, 4,4,4)
    return x,y,z


def __list_2d_write_prepare(l):
    s = ''
    for dim in l:
        line = ' '.join(str(item) for item in dim)
        s += line + '\n'
    return s


def cam_callback(event, path, queue):
    print 'Starting TOF...'
    times = []

    cam=SrCam()
    res=cam.ReadSerial()
    #print 'Serial Number:',hex(res)
    cam.OpenDlg(1,0)
    time.sleep(0.1)
    cam.SetModulationFrequency(SrCam.ModulationFrq['MF_30MHz'])
    time.sleep(0.1)
    print(cam.GetMode())
    #???print(cam.GetMode() | SrCam.AcquireMode['AM_DENOISE_ANF'])
    #cam.SetMode(cam.GetMode() | SrCam.AcquireMode['AM_DENOISE_ANF'])
    cam.SetMode(0x0911)
    time.sleep(0.1)
    print(cam.GetMode())

#    it = 50
#    while cam.GetIntegrationTime() != it:
#        cam.SetIntegrationTime(it)
#        time.sleep(0.01)

    print 'TOF: IT Ok'

#    integrationTime=cam.GetIntegrationTime()
#    amplitudeTreshold=cam.GetAmplitudeThreshold()
#    modulationFrequency=cam.GetModulationFrequency()

#    with open('{}parameters.csv'.format(path), 'wb') as x:
#        writer = csv.writer(x,delimiter=';')
#        writer.writerow([integrationTime,amplitudeTreshold,modulationFrequency])


    i = 0
    while not event.is_set():
        cam.Acquire()

        modulationFrequency=cam.GetModulationFrequency()
        integrationTime=cam.GetIntegrationTime()
        times.append([i,time.clock(),integrationTime,modulationFrequency])

        #distance = cam.GetImage(SrCam.ImgType['IT_DISTANCE'])
        #amplitude = cam.GetImage(SrCam.ImgType['IT_AMPLITUDE'])
        #confidence = cam.GetImage(SrCam.ImgType['IT_CONF_MAP'])
        res, ie = cam.GetImageList()
        for idx in range(res):
            #print ie[idx].imgType
            #print 'idx',idx,':',
            #for attr in ie[idx]._fields_:
            #    print attr[0],':',ie[idx].__getattribute__(attr[0]),
            #print(cam.GetImage(idx))
            if ie[idx].imgType == SrCam.ImgType['IT_AMPLITUDE']:
                amplitude = cam.GetImage(idx)
            elif ie[idx].imgType == SrCam.ImgType['IT_CONF_MAP']:
                confidence = cam.GetImage(idx)

        x, y, z = __get_coord_flt(cam)

        filename = "{}{}ToF.npz".format(path,i)
        queue.put((filename, copy(x), copy(y), copy(z), copy(amplitude), copy(confidence)))

        i+=1
        time.sleep(0.001)  # not ~10Hz
        with open('{}timestamp.csv'.format(path), 'ab') as f:
            writer = csv.writer(f,delimiter=';')
            writer.writerow(times)

    cam.Close()


def files_worker(event, queue):
    while not queue.empty() or not event.is_set():
        if event.is_set():
            print("remaining TOF files to save: {}".format(queue.qsize()))
        data = queue.get(timeout=1)
        path = data[0]

        #buff = ''
        li = []
        for item in data[1:]:
            li.append(item)
        savez(path, li)
            #buff += __list_2d_write_prepare(item)
        #with open(path,"w+") as f:
        #    f.write(buff)
    print('TOF files saved...')

class ToF(object):

    def __init__(self, path):
        self.event = Event()
        self.queue = Queue()
        self.thread = Thread(target=cam_callback, args=(self.event, path, self.queue))
        self.file_thread = Thread(target=files_worker, args=(self.event, self.queue))

    def start(self):
        if self.thread.is_alive() or self.file_thread.is_alive():
            print('Camera acquisition already started')
            return
        self.file_thread.start()
        self.thread.start()

    def stop(self):
        if not self.thread.is_alive() and not self.file_thread.is_alive():
            print('Camera acquisition not started yet')
            return
        self.event.set()
        self.file_thread.join()
        self.thread.join()
        self.event.clear()
        print 'TOF Stopped...'