import time
import microcontroller
import digitalio

class SWD:
    def __init__(swdio, swclk):
        self.swdio = swdio
        self.swclk = swclk

    def swd_setup(self):
        self.swclk = digitalio.DigitalInOut(swclk)
        self.swclk.switch_to_output()
        self.swclk.value = 1
        self.swdio = digitalio.DigitalInOut(swdio)
        self.swdio.switch_to_output()
        self.swdio.value = 1

    def sw_clk_cycle(self):
        self.swclk.value = 0
        microontroller.delay_us(1)
        self.swclk.value = 1
        microcontroller.delay_us(1)

    def sw_write_bit(self, bit):
        self.swdio.switch_to_output()
        self.swdio.value = bit
        self.swclk.value = 0
        microcontroller.delay_us(1)
        self.swclk.value = 1
        microcontroller.delay_us(1)

    def sw_read_bit(self):
        self.swclk.value = 0
        microcontroller.delay_us(1)
        self.swdio.switch_to_input()
        bit = swdio.value
        self.swclk.value = 1
        microcontroller.delay_us(1)
        return bit

    def swd_transfer(self, request, data):
        parity = 0
        self.sw_write_bit(1)
        bit = request >> 0
        self.sw_write_bit(bit)
        parity += bit
        bit = request >> 1
        self.sw_write_bit(bit)
        parity += bit
        bit = request >> 2
        self.sw_write_bit(bit)
        parity += bit
        bit = request >> 3
        self.sw_write_bit(bit)
        parity += bit
        self.sw_write_bit(parity)
        self.sw_write_bit(0)
        self.sw_write_bit(1)