def scroll(use_spi=False, soft=True):
    ssd = setup(use_spi, soft)  # Create a display instance
    rhs = WIDTH -1
    ssd.line(rhs - 20, 0, rhs, 20, 1)
    square_side = 10
    ssd.fill_rect(rhs - square_side, 0, square_side, square_side, 1)

    Writer.set_textpos(ssd, 0, 0)  # In case previous tests have altered it
    wri = Writer(ssd, freesans20, verbose=False)
    wri.set_clip(False, False, False)  # Char wrap
    wri.printstring('Sunday\n')
    wri.printstring('12 Aug 2018\n')
    wri.printstring('10.30am')
    for x in range(5):
        ssd.show()
        utime.sleep(2)
        wri.printstring('\nCount = {:2d}'.format(x))
    ssd.show()
    utime.sleep(2)
    wri.printstring('\nDone.')
    ssd.show() 

def usd_scroll(use_spi=False, soft=True):
    ssd = setup(use_spi, soft)  # Create a display instance
    # Only CWriter can do usd
    CWriter.invert_display(ssd)
    CWriter.set_textpos(ssd, 0, 0)
    wri = CWriter(ssd, freesans20, verbose=False)
    wri.set_clip(False, False, False)  # Char wrap

    wri.printstring('Sunday\n')
    wri.printstring('12 Aug 2018\n')
    wri.printstring('10.30am')
    for x in range(5):
        ssd.show()
        utime.sleep(2)
        wri.printstring('\nCount = {:2d}'.format(x))
    ssd.show()
    utime.sleep(2)
    wri.printstring('\nDone.')
    ssd.show()
    CWriter.invert_display(ssd, False)  # For subsequent tests 

def multi_fields(t):
    print('Dynamic labels.')
    refresh(ssd, True)  # Clear any prior image
    nfields = []
    dy = wri.height + 6
    y = 2
    col = 15
    width = wri.stringlen('99.99')
    for txt in ('X:', 'Y:', 'Z:'):
        Label(wri, y, 0, txt)  # Use wri default colors
        nfields.append(Label(wri, y, col, width, bdcolor=None))  # Specify a border, color TBD
        y += dy

    end = utime.ticks_add(utime.ticks_ms(), t * 1000)
    while utime.ticks_diff(end, utime.ticks_ms()) > 0:
        for field in nfields:
            value = int.from_bytes(uos.urandom(3),'little')/167772
            overrange =  None if value < 70 else YELLOW if value < 90 else RED
            field.value('{:5.2f}'.format(value), fgcolor = overrange, bdcolor = overrange)
        refresh(ssd)
        utime.sleep(1)
    Label(wri, 0, 64, ' OK ', True, fgcolor = RED)
    refresh(ssd)
    utime.sleep(1) 

def multi_fields(t):
    print('multi_fields')
    refresh(ssd, True)  # Clear any prior image
    nfields = []
    dy = wri.height + 6
    y = 2
    col = 15
    width = wri.stringlen('99.99')
    for txt in ('X:', 'Y:', 'Z:'):
        Label(wri, y, 0, txt)  # Use wri default colors
        nfields.append(Label(wri, y, col, width, bdcolor=None))  # Specify a border, color TBD
        y += dy

    end = utime.ticks_add(utime.ticks_ms(), t * 1000)
    while utime.ticks_diff(end, utime.ticks_ms()) > 0:
        for field in nfields:
            value = int.from_bytes(uos.urandom(3),'little')/167772
            overrange =  None if value < 70 else YELLOW if value < 90 else RED
            field.value('{:5.2f}'.format(value), fgcolor = overrange, bdcolor = overrange)
        refresh(ssd)
        utime.sleep(1)
    Label(wri, 0, 64, ' OK ', True, fgcolor = RED)
    refresh(ssd)
    utime.sleep(1) 

def fields(use_spi=False, soft=True):
    ssd = setup(use_spi, soft)  # Create a display instance
    Writer.set_textpos(ssd, 0, 0)  # In case previous tests have altered it
    wri = Writer(ssd, fixed, verbose=False)
    wri.set_clip(False, False, False)
    textfield = Label(wri, 0, 2, wri.stringlen('longer'))
    numfield = Label(wri, 25, 2, wri.stringlen('99.99'), bdcolor=None)
    countfield = Label(wri, 0, 90, wri.stringlen('1'))
    n = 1
    for s in ('short', 'longer', '1', ''):
        textfield.value(s)
        numfield.value('{:5.2f}'.format(int.from_bytes(uos.urandom(2),'little')/1000))
        countfield.value('{:1d}'.format(n))
        n += 1
        refresh(ssd)
        utime.sleep(2)
    textfield.value('Done', True)
    refresh(ssd) 

def multi_fields(use_spi=False, soft=True):
    ssd = setup(use_spi, soft)  # Create a display instance
    Writer.set_textpos(ssd, 0, 0)  # In case previous tests have altered it
    wri = Writer(ssd, small, verbose=False)
    wri.set_clip(False, False, False)

    nfields = []
    dy = small.height() + 6
    y = 2
    col = 15
    width = wri.stringlen('99.99')
    for txt in ('X:', 'Y:', 'Z:'):
        Label(wri, y, 0, txt)
        nfields.append(Label(wri, y, col, width, bdcolor=None))  # Draw border
        y += dy

    for _ in range(10):
        for field in nfields:
            value = int.from_bytes(uos.urandom(3),'little')/167772
            field.value('{:5.2f}'.format(value))
        refresh(ssd)
        utime.sleep(1)
    Label(wri, 0, 64, ' DONE ', True)
    refresh(ssd) 

def wifi_connect(essid, password):
    # Connect to the wifi. Based on the example in the micropython
    # documentation.
    wlan = network.WLAN(network.STA_IF)
    wlan.active(True)
    if not wlan.isconnected():
        print('connecting to network ' + essid + '...')
        wlan.connect(essid, password)
        # connect() appears to be async - waiting for it to complete
        while not wlan.isconnected():
            print('waiting for connection...')
            utime.sleep(4)
            print('checking connection...')
        print('Wifi connect successful, network config: %s' % repr(wlan.ifconfig()))
    else:
        # Note that connection info is stored in non-volatile memory. If
        # you are connected to the wrong network, do an explicity disconnect()
        # and then reconnect.
        print('Wifi already connected, network config: %s' % repr(wlan.ifconfig())) 

def suspend():
    for x in range(10, 0, -1):
        print('nunning', x)
        utime.sleep(0.5)
        wdt_feed(5)  # Hold off for 5s

    wdt_feed(WDT_SUSPEND)
    for x in range(5, 0, -1):
        print('suspended', x)
        utime.sleep(0.5)

    for x in range(5, 0, -1):
        print('nunning', x)
        utime.sleep(0.5)
        wdt_feed(5)  # Hold off for 5s

    print('Should reset in 5s')
    utime.sleep(10)
    wdt_feed(WDT_CANCEL)  # Should never execute

# Default period 

def find_heart_rate(self):
        """ """
        adv = self.bt.get_adv()
        if adv and self.bt.resolve_adv_data(adv.data, Bluetooth.ADV_NAME_CMPL) == 'Heart Rate':
            try:
                conn = self.bt.connect(adv.mac, timeout=5000)
                services = conn.services()
                for service in services:
                    utime.sleep(0.050)
                    if type(service.uuid()) == bytes:
                        print('Reading chars from service = {}'.format(service.uuid()))
                    else:
                        print('Reading chars from service = %x' % service.uuid())
                    chars = service.characteristics()
                    for char in chars:
                        if (char.properties() & Bluetooth.PROP_READ):
                            print('char {} value = {}'.format(char.uuid(), char.read()))
                conn.disconnect()
            except:
                print("Error while connecting or reading from the BLE device")
        else:
            utime.sleep(0.050)

        utime.sleep(10.0) 

def dual(use_spi=False, soft=True):
    ssd0 = setup(False, soft)  # I2C display
    ssd1 = setup(True, False)  # SPI  instance
    Writer.set_textpos(ssd0, 0, 0)  # In case previous tests have altered it
    wri0 = Writer(ssd0, small, verbose=False)
    wri0.set_clip(False, False, False)
    Writer.set_textpos(ssd1, 0, 0)  # In case previous tests have altered it
    wri1 = Writer(ssd1, small, verbose=False)
    wri1.set_clip(False, False, False)

    nfields = []
    dy = small.height() + 6
    col = 15
    for n, wri in enumerate((wri0, wri1)):
        nfields.append([])
        y = 2
        for txt in ('X:', 'Y:', 'Z:'):
            Label(wri, y, 0, txt)
            nfields[n].append(Label(wri, y, col, wri.stringlen('99.99'), True))
            y += dy

    for _ in range(10):
        for n, wri in enumerate((wri0, wri1)):
            for field in nfields[n]:
                value = int.from_bytes(uos.urandom(3),'little')/167772
                field.value('{:5.2f}'.format(value))
            wri.device.show()
            utime.sleep(1)
    for wri in (wri0, wri1):
        Label(wri, 0, 64, ' DONE ', True)
        wri.device.show() 

def meter():
    print('Meter test.')
    refresh(ssd, True)  # Clear any prior image
    color = lambda v : RED if v > 0.7 else YELLOW if v > 0.5 else GREEN
    txt = lambda v : 'ovr' if v > 0.7 else 'high' if v > 0.5 else 'ok'
    m0 = Meter(wri, 5, 2, divisions = 4, ptcolor=YELLOW,
              label='left', style=Meter.BAR, legends=('0.0', '0.5', '1.0'))
    l0 = LED(wri, ssd.height - 16 - wri.height, 2, bdcolor=YELLOW, label ='over')
    m1 = Meter(wri, 5, 50, divisions = 4, ptcolor=YELLOW,
              label='right', style=Meter.BAR, legends=('0.0', '0.5', '1.0'))
    l1 = LED(wri, ssd.height - 16 - wri.height, 50, bdcolor=YELLOW, label ='over')
    m2 = Meter(wri, 5, 98, divisions = 4, ptcolor=YELLOW,
              label='bass', style=Meter.BAR, legends=('0.0', '0.5', '1.0'))
    l2 = LED(wri, ssd.height - 16 - wri.height, 98, bdcolor=YELLOW, label ='over')
    steps = 10
    for n in range(steps):
        v = int.from_bytes(uos.urandom(3),'little')/16777216
        m0.value(v, color(v))
        l0.color(color(v))
        l0.text(txt(v), fgcolor=color(v))
        v = n/steps
        m1.value(v, color(v))
        l1.color(color(v))
        l1.text(txt(v), fgcolor=color(v))
        v = 1 - n/steps
        m2.value(v, color(v))
        l2.color(color(v))
        l2.text(txt(v), fgcolor=color(v))
        refresh(ssd)
        utime.sleep(1) 

def vari_fields():
    print('Variable label styles.')
    refresh(ssd, True)  # Clear any prior image
    wri_large = CWriter(ssd, freesans20, GREEN, BLACK, verbose=False)
    wri_large.set_clip(True, True, False)
    Label(wri_large, 0, 0, 'Text')
    Label(wri_large, 20, 0, 'Border')
    width = wri_large.stringlen('Yellow')
    lbl_text = Label(wri_large, 0, 65, width)
    lbl_bord = Label(wri_large, 20, 65, width)
    lbl_text.value('Red')
    lbl_bord.value('Red')
    lbl_var = Label(wri_large, 50, 2, '25.46', fgcolor=RED, bdcolor=RED)
    refresh(ssd)
    utime.sleep(2)
    lbl_text.value('Red')
    lbl_bord.value('Yellow')
    lbl_var.value(bdcolor=YELLOW)
    refresh(ssd)
    utime.sleep(2)
    lbl_text.value('Red')
    lbl_bord.value('None')
    lbl_var.value(bdcolor=False)
    refresh(ssd)
    utime.sleep(2)
    lbl_text.value('Yellow')
    lbl_bord.value('None')
    lbl_var.value(fgcolor=YELLOW)
    refresh(ssd)
    utime.sleep(2)
    lbl_text.value('Blue')
    lbl_bord.value('Green')
    lbl_var.value('18.99', fgcolor=BLUE, bdcolor=GREEN)
    Label(wri, ssd.height - wri.height - 2, 0, 'Done', fgcolor=RED)
    refresh(ssd) 

def meter():
    print('meter')
    refresh(ssd, True)  # Clear any prior image
    m = Meter(wri, 5, 2, height = 45, divisions = 4, ptcolor=YELLOW,
              label='level', style=Meter.BAR, legends=('0.0', '0.5', '1.0'))
    l = LED(wri, 5, 40, bdcolor=YELLOW, label ='over')
    steps = 10
    for _ in range(steps):
        v = int.from_bytes(uos.urandom(3),'little')/16777216
        m.value(v)
        l.color(GREEN if v < 0.5 else RED)
        refresh(ssd)
        utime.sleep(1)
    refresh(ssd) 

def meter(use_spi=False, soft=True):
    ssd = setup(use_spi, soft)
    wri = Writer(ssd, arial10, verbose=False)
    ssd.fill(0)
    refresh(ssd)
    m0 = Meter(wri, 5, 2, height = 50, divisions = 4, legends=('0.0', '0.5', '1.0'))
    m1 = Meter(wri, 5, 44, height = 50, divisions = 4, legends=('-1', '0', '+1'))
    m2 = Meter(wri, 5, 86, height = 50, divisions = 4, legends=('-1', '0', '+1'))
    steps = 10
    for n in range(steps + 1):
        m0.value(int.from_bytes(uos.urandom(3),'little')/16777216)
        m1.value(n/steps)
        m2.value(1 - n/steps)
        refresh(ssd)
        utime.sleep(1) 

def aclock():
    uv = lambda phi : cmath.rect(1, phi)  # Return a unit vector of phase phi
    pi = cmath.pi
    days = ('Monday', 'Tuesday', 'Wednesday', 'Thursday', 'Friday', 'Saturday',
            'Sunday')
    months = ('Jan', 'Feb', 'March', 'April', 'May', 'June', 'July',
              'Aug', 'Sept', 'Oct', 'Nov', 'Dec')
    # Instantiate CWriter
    CWriter.set_textpos(ssd, 0, 0)  # In case previous tests have altered it
    wri = CWriter(ssd, arial10, GREEN, BLACK, verbose=False)
    wri.set_clip(True, True, False)

    # Instantiate displayable objects
    dial = Dial(wri, 2, 2, height = 75, ticks = 12, bdcolor=None, label=120, pip=False)  # Border in fg color
    lbltim = Label(wri, 5, 85, 35)
    hrs = Pointer(dial)
    mins = Pointer(dial)
    secs = Pointer(dial)

    hstart =  0 + 0.7j  # Pointer lengths and position at top
    mstart = 0 + 0.92j
    sstart = 0 + 0.92j 
    while True:
        t = utime.localtime()
        hrs.value(hstart * uv(-t[3]*pi/6 - t[4]*pi/360), YELLOW)
        mins.value(mstart * uv(-t[4] * pi/30), YELLOW)
        secs.value(sstart * uv(-t[5] * pi/30), RED)
        lbltim.value('{:02d}.{:02d}.{:02d}'.format(t[3], t[4], t[5]))
        dial.text('{} {} {} {}'.format(days[t[6]], t[2], months[t[1] - 1], t[0]))
        refresh(ssd)
        utime.sleep(1) 

def move(self, speed, duration=5):
        self.left = -speed
        self.right = -speed
        # time.sleep(duration)
        # self.stop() 

def turn(self, speed, duration=5):
        if speed > 0: # Turn RIGHT
            self.left = abs(speed)
            self.right = 0
        elif speed < 0: # Turn LEFT
            self.left = 0
            self.right = abs(speed)
        # time.sleep(duration)
        # self.stop() 

def rotate(self, speed, duration=2):
        if speed > 0:
            self.left = speed
            self.right = -speed
        elif speed < 0:
            self.left = speed
            self.right = -speed
        # time.sleep(duration)
        # self.stop() 

def __init__(self):
        self.uart = machine.UART(1, tx=17, rx=16)
        self.uart.init(9600, bits=8, parity=None, stop=1)
        self.callback = None
        if self._reset() == 1:
            raise module.Module('Module lorawan not connect')
        self._timer = None
        self.uartString = ''
        time.sleep(2) 

def _waitTimeout(self, t):
        t1 = time.ticks_ms()
        while True:
            time.sleep(0.01)
            if self.uart.any():
                return 0
            if time.ticks_ms() - t1 > t:
                return 1 

def readUid(self):
        if self._request(0x26)[0] != OK:
            time.sleep(0.005)
            if self._request(0x26)[0] != OK:
                return ''
        
        data_str = ''
        data = self._anticoll()[1]
        for i in data:
            data_str += str(hex(i))[2:]
        
        return data_str 

def init(self):
        self._reset()
        time.sleep(0.05)
        self._wreg(0x2A, 0x80)
        self._wreg(0x2B, 0xA9)
        self._wreg(0x2C, 0x03)
        self._wreg(0x2D, 0xE8)
        self._wreg(0x15, 0x40)
        self._wreg(0x11, 0x3D)
        self._antenna_on() 

def _get_access(self, block, keyA=None, keyB=None):
        if self._request(0x26)[0] != OK:
            time.sleep(0.005)
            if self._request(0x26)[0] != OK:
                return None, 1
        
        stat, uid = self._anticoll()
        if stat == ERR:
            return None, 2

        if self._selectTag(uid) == ERR:
            return None, 3

        key = [0xff] * 6
        mode = AUTHENT1A

        if keyA:
            key = keyA
            mode = AUTHENT1A
        elif keyB:
            key = keyB
            mode = AUTHENT1B

        if self._auth(mode, block, key, uid) == ERR:
            return None, 4
        
        return OK, 0 

def isCardOn(self):
        if self._request(0x26)[0] != OK:
            time.sleep(0.005)
            if self._request(0x26)[0] != OK:
                return False
        return True 

def readUid(self):
        if self._request(0x26)[0] != OK:
            time.sleep(0.005)
            if self._request(0x26)[0] != OK:
                return ''
        
        data_str = ''
        data = self._anticoll()[1]
        for i in data:
            data_str += str(hex(i))[2:]
        
        return data_str 

def _get_access(self, block, keyA=None, keyB=None):
        if self._request(0x26)[0] != OK:
            time.sleep(0.005)
            if self._request(0x26)[0] != OK:
                return None, 1
        
        stat, uid = self._anticoll()
        if stat == ERR:
            return None, 2

        if self._selectTag(uid) == ERR:
            return None, 3

        key = [0xff] * 6
        mode = AUTHENT1A

        if keyA:
            key = keyA
            mode = AUTHENT1A
        elif keyB:
            key = keyB
            mode = AUTHENT1B

        if self._auth(mode, block, key, uid) == ERR:
            return None, 4
        
        return OK, 0 

def enable(self):
        self._write_u8(_REGISTER_ENABLE, _ENABLE_PON)
        time.sleep(0.003)
        self._write_u8(_REGISTER_ENABLE, _ENABLE_PON | _ENABLE_AEN)
        time.sleep(0.154)

    # def color_near(self):
    #     pass 

def get_next_command(self):
  while True:
   try:
    payload=self.socket.recv(self.packet_len)
    pkt_len_received=len(payload)
    print(pkt_len_received)
    while pkt_len_received<self.packet_len:
     wait_for=self.packet_len-pkt_len_received
     short_packet=self.socket.recv(wait_for)
     payload+=short_packet
     pkt_len_received+=len(payload)
   except OSError:
    if self.adc:
     adc_val=self.adc.read()
     if abs(self.last_adc-adc_val)>=self.adc_diff_report:
      self.send_payload_to_gateway({'report':'analog_input','pin':0,'value':adc_val})
      self.last_adc=adc_val
    if self.sonar:
     dist=self.sonar.distance_in_cm()
     payload={'report':'sonar_data','value':dist}
     self.send_payload_to_gateway(payload)
     utime.sleep(.05)
    continue
   try:
    payload=ujson.loads(payload)
   except ValueError:
    self.send_payload_to_gateway({'error':'json value error - data: {} length {}'.format(payload,len(payload))})
    self.socket.close()
    break
   command=payload['command']
   if command in self.command_dictionary.keys():
    self.command_dictionary[command](payload)
   else:
    self.additional_banyan_messages(payload) 

def normal():
    try:
        for x in range(10, 0, -1):
            print('nunning', x)
            utime.sleep(0.5)
            wdt_feed(5)  # Hold off for 5s

        print('Should reset in 5s')
        utime.sleep(10)
    except KeyboardInterrupt:
        pass
    finally:
        wdt_feed(WDT_CANCEL)  # Should never execute

# Suspend and resume 

def default():
    for x in range(10, 0, -1):
        print('nunning', x)
        utime.sleep(0.5)
        wdt_feed(5)  # Hold off for 5s

    wdt_feed(0)  # Use default period
    print('Should reset in 2s')
    utime.sleep(10)
    wdt_feed(WDT_CANCEL)  # Should never execute

# Cancellation