import serial
import time
import threading

info = { 'count':0,
                'forward_dist':0,
                'right_dist':0,
                'back_dist':0,
                'left_dist':0,
                'light':0 }

class SerialTread (threading.Thread):
    c = 0
    def __init__(self, name, counter):
        threading.Thread.__init__(self)
        self.threadID = counter
        self.name = name
        self.counter = counter
    def run(self):        
        while self.c == 0:
            line = str(ser.readline())[4:-3].split()
            info['count'] = int(line[0][2:])
            info['forward_dist'] = int(line[1][6:])
            info['right_dist'] = int(line[2][6:])
            info['back_dist'] = int(line[3][6:])
            info['left_dist'] = int(line[4][6:])
            info['light'] = int(line[5][6:])
    def join(self):
        self.c = 1

class TelloModule():
    def __init__(self):
        """Constructor"""
        pass

    def module_init(self, module):
        global type_module
        global t
        global ser

        t = SerialTread("Thread", 1)
        ser = serial.Serial('/dev/ttyUSB0', 115200)
        ser.readline()

        type_module = module   
        if type_module == 'distance':        
            t.start()
            time.sleep(1)
        else:
            print ("Error: There is NO module with this name, please select \"distance\".")



    def module_deinit(self):
        if type_module == 'distance':        
            t.join()
            type_module == ''
        else:
            print ("Error: First you need to initialize the module.")



    def get_forward_dist(self):
        return info['forward_dist']

    def get_left_dist(self):
        return info['left_dist']

    def get_right_dist(self):
        return info['right_dist']

    def get_back_dist(self):
        return info['back_dist']

    def get_light(self):
        return info['light']

    def get_count(self):
        return info['count']

    def get_dist(self):
        return [info['forward_dist'], info['right_dist'], info['left_dist'], info['back_dist']]

    def get_block_dist(self):           # count + forward + right + back + left + light 
        ser = serial.Serial('/dev/ttyUSB0', 115200)
        line = str(ser.readline())[4:-3].split()
        ser.close()
        if line[0][3] == "r" :
            return [int(line[0][6:]), int(line[1][6:]), int(line[2][6:]), int(line[3][6:]), int(line[4][6:]), int(line[5][6:])]
        else:
            return [int(line[0]), int(line[1][6:]), int(line[2][6:]), int(line[3][6:]), int(line[4][6:]), int(line[5][6:])]


if __name__ == "__main__":
    mod = TelloModule()
    
    print ("\n -------- Test blocking mode --------- \n")
    for i in range(10):
        line = mod.get_block_dist()
        print ("Test " + str(i) +
                " count = " + str(line[0]) + 
                " forward = " + str(line[1]) + 
                " right = " + str(line[2]) + 
                " back = " + str(line[3]) + 
                " left = " + str(line[4]) + 
                " light = " + str(line[5]))

    print ("\n ------ Test NON Blocking mode ------- \n")
    mod.module_init('distance')
    for i in range(10):
        print ("Test " + str(i) +
                " count = " + str(mod.get_count()) + 
                " forward = " + str(mod.get_forward_dist()) + 
                " right = " + str(mod.get_right_dist()) + 
                " back = " + str(mod.get_back_dist()) + 
                " left = " + str(mod.get_left_dist()) + 
                " light = " + str(mod.get_light()))
        time.sleep(0.5)
    mod.module_deinit()

    print ("\n ------------ Finish Test ------------ \n")