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

class Plotter:
    def __init__(self, config, eventQ: Queue, runningEvent: Event, scannerLock: Lock):
        #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")
        self.scannerLock = scannerLock
        self.goPark = False
        self.locked = False
        self.ad = None

    def park(self):
        self.logger.info("Queue to park plotter")
        self.goPark = True
        #self.q.put([(1/2.54)/self.plotWidth + 1,0,0])
        absPlotWidth = 26.5/2.54
        topLeft = absPlotWidth / self.plotWidth #ignore changes in config plotwidth
        self.q.put([(1/2.54)/absPlotWidth + topLeft,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.options.pen_pos_up = 100

                self.park()
#                 self.ad.moveto(0,0)

            else:
                self.ad = None
            self.axiDrawCueListener()
        except Exception as e:
            self.logger.exception(e)
        finally:
            self.logger.warning("Close Axidraw connection")
            if self.ad:
                try:
                    with self.scannerLock:
                        self.ad.moveto(0,0)
                        self.ad.disconnect()
                except Exception as e:
                    self.logger.warning("Error on closing axidraw:")
                    self.logger.exception(e)

            self.logger.info("Clear running Event")
            # send shutdown signal (if not already set)
            self.isRunning.clear()

    def draw_segments(self, segments = []):
        if not self.locked:
            # acquire lock if not already done so
            self.scannerLock.acquire()
            self.locked = True

        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:
            if len(coordinates) < 2:
                self.logger.info("Plot single point (park?)")
                self.ad.moveto(coordinates[0][0], coordinates[0][1])
            else:
                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)

                #if self.goPark:
                #  print("seg",segment)

                # 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):
                        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'))
                    if self.goPark:
                        self.eventQ.put(Signal('plotter.parked'))
                        if self.locked:
                            self.scannerLock.release()
                            self.locked = False
                        self.goPark = False
#                 else:
#                     time.sleep(.05)
#                     self.logger.debug(f'Plotter move: {move}')
        self.logger.info("Stopping plotter")