import queue
import logging
from pyaxidraw import axidraw
from queue import Queue
from threading import Event
from sorteerhoed.Signal import Signal
import time

class Plotter:
    def __init__(self, config, eventQ: Queue, runningEvent: Event):
        #TODO: scanningEvent -> CentralManagement.isScanning -> prevent plotter move during scan, failsafe
        self.config = config
        self.eventQ = eventQ
        self.q = Queue()
        self.isRunning = runningEvent
        self.logger = logging.getLogger("sorteerhoed").getChild("plotter")
        self.pen_down = False
        
        self.plotWidth = self.config['scanner']['width'] / 10 / 2.54
        self.plotHeight = self.config['scanner']['height'] / 10 / 2.54
        self.xPadding = self.config['scanner']['left_padding'] / 10 / 2.54;
        self.yPadding = self.config['scanner']['top_padding'] / 10 / 2.54;
        self.logger.info(f"Paddings x: {self.xPadding} inch y: {self.yPadding} inch")
        
    def park(self):
        self.logger.info("Queue to park plotter")
        
        if self.config['dummy_plotter']:
            # TODO: find a nice way to park the axidraw in the 0 position
            self.q.put([(-1/2.54)/self.plotWidth,0,0])
        
    def start(self):
        try:
            if not self.config['dummy_plotter']:
                self.ad = axidraw.AxiDraw()
                    
                self.ad.interactive()
        #             self.ad.plot_path()
                
                connected = self.ad.connect()
                if not connected:
                    raise Exception("Cannot connect to Axidraw")
                
                self.ad.options.units = 1 # set to use centimeters instead of inches
                self.ad.options.accel = 100;
                self.ad.options.speed_penup = 100
                self.ad.options.speed_pendown = 100
                self.ad.options.model = 1 # 2 for A3, 1 for A4
                
                self.ad.moveto(0,0)
                
#                 plotterWidth = 25
#                 plotterHeight = 21
                
            else:
                self.ad = None
            self.axiDrawCueListener()
        except Exception as e:
            self.logger.exception(e)
        finally:
            self.logger.warning("Close Axidraw connection")
            if self.ad:
                self.ad.moveto(0,0)
                self.ad.disconnect()
            
            # send shutdown signal (if not already set)
            self.isRunning.clear()
    
    def draw_segments(self, segments = []):
        coordinates = []
        for segment in segments:
            coordinate = [
                 # mm to cm to inches
                (segment[0]) * self.plotWidth,
                (1-segment[1]) * self.plotHeight
            ]
            #prevent drawing when not in drwaing area
            # this is a failsafe for a malicious working or glitching script, as this should also be done in the javascript
            if self.pen_down:
                if coordinate[0] < self.xPadding or coordinate[0] > self.xPadding+self.plotWidth or \
                    coordinate[1] < self.yPadding or coordinate[1] > self.yPadding + self.plotHeight:
                    self.logger.warn(f"Skip drawing for: {coordinates} out of bounds")
                    continue 
            
            coordinates.append(coordinate)
        self.logger.debug(f"Plot: {coordinates}")
        if self.ad:
            self.ad.plan_trajectory(coordinates)
#         self.ad.moveto(move[0]* plotterWidth, move[1]*plotterHeight)
#                         self.logger.debug(f'handler! {move}')
        pass
    
    def setPenDown(self, pen_state):
        """
        False: pen raised, True: pen_lower
        """
        if pen_state != self.pen_down:
            self.pen_down = pen_state
            self.logger.info("Changed pen: {}".format('down' if pen_state else 'up'))
            if self.ad:
                if pen_state:
                    self.ad.pen_lower()
                else:
                    self.ad.pen_raise()
            return True
        return False

    def axiDrawCueListener(self):
        plotterRan = False
        segments = []
        while self.isRunning.is_set():
            # TODO: queue that collects a part of the path data
            # on a specific limit or on a specific time interval, do the plot
            # also, changing ad.pen_raise() or ad.pen_lower() trigger a new segment
            # Plot with ad.plan_trajectory() ??
            try:
                # if no info comes in for .5sec, plot that segment
                segment = self.q.get(True, .5)
                
                # change of pen state? draw previous segments!
                if (segment[2] == 1 and not self.pen_down) or (segment[2] == 0 and self.pen_down) or len(segments) > 150:
                    if len(segments) > 1:
                        self.draw_segments(segments)
                        plotterRan = True
                    segments = [] #reset
                    
                    # and change pen positions
                    self.setPenDown(segment[2] == 1)
                    

                segments.append(segment)
                
            except queue.Empty as e:
                self.logger.debug("Timeout queue.")
                if len(segments):
                    # segments to plot!
                    self.draw_segments(segments)
                    plotterRan = True
                    segments = []
                elif plotterRan:
                    plotterRan = False
                    self.eventQ.put(Signal('plotter.finished'))
#                 else:
#                     time.sleep(.05)
#                     self.logger.debug(f'Plotter move: {move}')
        self.logger.info("Stopping plotter")