import threading
import time

from .DustCleaner import DustCleaner

# # add python path of src to sys.path
# src_path = os.path.join(__file__, *(['..'] * 2))
# src_path = os.path.abspath(src_path)
# sys.path.insert(0, src_path)

from ..protos import aerial_pb2 as pb



class DustCleanerThread (threading.Thread):
    """
    dust cleaner threads
    """
    def __init__(self, cmd_queue, config):
        threading.Thread.__init__(self)
        self.config = config
        self.cmd_queue = cmd_queue
        self.stop = False
        self.cleaner = DustCleaner("com11", 115200, 1)
        self._init_dust_cleaner()

    def _init_dust_cleaner(self):
        """init dust cleaner"""
        # send dust cleaner self clean command
        self.cleaner.Send_self_clean_params(
            self.config.clean_self_start_angle,
            self.config.clean_self_end_angle,
            self.config.clean_self_num)
        # send dust cleaner period clean self command
        self.cleaner.Send_period_clean_self(self.config.clean_self_period)
        # send dust cleaner camera clean command
        self.cleaner.Send_camera_clean_params(
            self.config.clean_lens_start_angle,
            self.config.clean_lens_end_angle,
            self.config.clean_lens_num)
        # send dust cleaner period clean camera command
        self.cleaner.Send_period_clean_camera(self.config.clean_lens_period)

    def run(self):
        print("DustCleanerThread started")
        while not self.stop:
            # check if there is a command in the queue
            if not self.cmd_queue.empty():
                cmd = self.cmd_queue.get()
                dust_cleaner_msg = pb.DustCleanerMessage()
                dust_cleaner_msg.ParseFromString(cmd.data1.data)
                if cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_ONETIME_CLEAN_SELF:
                    self.cleaner.Send_onetime_clean_self()
                elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_ONETIME_CLEAN_CAMERA:
                    self.cleaner.Send_onetime_clean_camera()
                elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_CLOSE_PERIOD_CLEAN_SELF:
                    self.cleaner.Send_close_period_clean_self()
                elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_CLOSE_PERIOD_CLEAN_CAMERA:
                    self.cleaner.Send_close_period_clean_camera()
                elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_PERIOD_CLEAN_SELF:
                    self.cleaner.Send_close_period_clean_self(dust_cleaner_msg.period)
                elif cmd.sub_command == pb.ControlSubCommand.DUST_CLEANER_PERIOD_CLEAN_CAMERA:
                    self.cleaner.Send_period_clean_camera(dust_cleaner_msg.period)
            time.sleep(0.1)
        print("DustCleanerThread stopped")