import time

from serial import Serial


class EZSV23():

    def __init__(self, COM='COM4', baud=9600, timeout=0.05):
        self._fd = None     # serial port file descriptor
        self.open(COM, baud, timeout)


    def __enter__(self):
        return self


    def __exit__(self, type, value, traceback):
        self.close()


    def open(self, COM, baud, timeout):         # open and configure the device
        self._fd = Serial(COM, baud, timeout=timeout)

        self._write('m40u1000L20000N1V280000R')  # current limit, overload TO, accel, mode, velocity
        time.sleep(0.1)

        self._write('w1000x0y2500R')       # PID gains
        time.sleep(0.1)


    def close(self):                        # close the device
        if self._fd:
            self._fd.close()


    def tell_position(self):                # current position, in ticks
        reply = self._write('?8')
        position = reply[ reply.find(b'/0') + 3 : -3] # discard header and trailer
        return str(position, 'utf-8')                 # convert to str and return


    def tell_status(self):                  # read the status: [ready flag, error code]
        reply = self._write('Q')
        status_byte = reply[reply.find(b'/0') + 2]  # the status byte is 2 bytes after '/0'
        ready_flag = bool(status_byte & 32)         # ready flag is in bit 5 (high = ready, low = busy)
        error_code = status_byte & 15               # error code is in bits 0-3 --> see p.35 of docs for code interpretation
        return (ready_flag, error_code)           # (ready=bool, error=int)


    def home(self):                         # send the homing command
        self._write('gS03P10000G50Z700000R')  # in steps of 10000 ticks, at most 50 times, max after =700000 ticks


    def halt_execution(self):  # halt whatever program in executing
        self._write('T')


    def position_absolute(self, position):  # goto absolute position
        self._write('A' + position + 'R')


    def position_relative(self, position):  # goto relative position
        self._write('P' + position + 'R')


    def motor_off(self):                    # disable servo-control
        self._write('r0R')


    def wait_for_completion(self):
        ready = False
        while not ready:
            time.sleep(0.25)
            (ready,error) = self.tell_status()


    def wait_for_position(self, target, margin = 1000):
        ''' Returns when the position is within margin ticks of the target or the move is complete.'''
        target = int(target)
        there = False
        ready = False
        while not (there or ready):
            time.sleep(0.25)
            (ready,error) = self.tell_status()
            position = int(self.tell_position())
            if abs(position - target) < margin:
                there = True

                
    def servo_here(self):                   # enable servo-control
        self._write('r1R')


    def _write(self, command):            # send a command
        self._fd.reset_input_buffer()
        self._fd.write( b'/1' + bytes(command,'utf-8') + b'\r' )   # add ezv23 header/trailer and send
        reply = self._fd.readline()       # ezsv23 replies to every command; need to read it else the replies accumulate
        return reply