import sys
import time
import math
import setenv

from machine import Pin
if setenv.MOTOR_SOFT_UART == True:
    from softuart import UART
else:
    from machine import UART
#-----------------------------------------------------------------------
# this file contains:
# 1. hexadecimal address of the different registers
# 2. bitposition and bitmasks of the different values of each register
#
# Example:
# the register IOIN has the address 0x06 and the first bit shows
# whether the ENABLE (EN/ENN) Pin is currently HIGH or LOW
#-----------------------------------------------------------------------

#addresses
GCONF               = 0x00
GSTAT               = 0x01
IFCNT               = 0x02
IOIN                = 0x06
IHOLD_IRUN          = 0x10
TSTEP               = 0x12
TCOOLTHRS           = 0x14
SGTHRS              = 0x40
SG_RESULT           = 0x41
MSCNT               = 0x6A
CHOPCONF            = 0x6C
DRVSTATUS           = 0x6F
VACTUAL             = 0x22

#GCONF
i_scale_analog      = 1<<0
internal_rsense     = 1<<1
en_spreadcycle      = 1<<2
shaft               = 1<<3
index_otpw          = 1<<4
index_step          = 1<<5
mstep_reg_select    = 1<<7

#GSTAT
reset               = 1<<0
drv_err             = 1<<1
uv_cp               = 1<<2

#CHOPCONF
vsense              = 1<<17
msres0              = 1<<24
msres1              = 1<<25
msres2              = 1<<26
msres3              = 1<<27
intpol              = 1<<28

#IOIN
io_enn              = 1<<0
io_step             = 1<<7
io_spread           = 1<<8
io_dir              = 1<<9

#DRVSTATUS
stst                = 1<<31
stealth             = 1<<30
cs_actual           = 31<<16
t157                = 1<<11
t150                = 1<<10
t143                = 1<<9
t120                = 1<<8
olb                 = 1<<7
ola                 = 1<<6
s2vsb               = 1<<5
s2vsa               = 1<<4
s2gb                = 1<<3
s2ga                = 1<<2
ot                  = 1<<1
otpw                = 1<<0

#IHOLD_IRUN
ihold               = 31<<0
irun                = 31<<8
iholddelay          = 15<<16

#SGTHRS
sgthrs              = 255<<0

#others
mres_256            = 0
mres_128            = 1
mres_64             = 2
mres_32             = 3
mres_16             = 4
mres_8              = 5
mres_4              = 6
mres_2              = 7
mres_1              = 8


#-----------------------------------------------------------------------
# TMC_UART LogLevl
#-----------------------------------------------------------------------
class Loglevel():
    none = 0
    error = 10
    info = 20
    debug = 30
    all = 100


#-----------------------------------------------------------------------
# TMC_UART
#
# this class is used to communicate with the TMC via UART
# it can be used to change the settings of the TMC.
# like the current or the microsteppingmode
#-----------------------------------------------------------------------
class TMC_UART:
    mtr_id = 0
    ser = None
    rFrame = [0x55, 0, 0, 0]
    wFrame = [0x55, 0, 0, 0 , 0, 0, 0, 0]
    communication_pause = 0
    
#-----------------------------------------------------------------------
# constructor
#-----------------------------------------------------------------------
    def __init__(self, rx, tx, serialport, baudrate):
        if setenv.MOTOR_SOFT_UART == True:
            self.ser = UART(rx=rx, tx=tx, baudrate=baudrate, id=serialport)
            print("soft_tmc")
        else:
            self.ser = UART(serialport,baudrate,rx=Pin(rx), tx=Pin(tx))
            print("hard_tmc",serialport,baudrate,rx ,tx)
            
        self.mtr_id = 0
        #self.ser.timeout = 20000/baudrate            # adjust per baud and hardware. Sequential reads without some delay fail.
        self.communication_pause = 500 / baudrate     # adjust per baud and hardware. Sequential reads without some delay fail.

#-----------------------------------------------------------------------
# destructor
#-----------------------------------------------------------------------
    def deinit(self):
        self.ser.deinit()
       
#-----------------------------------------------------------------------
# this function calculates the crc8 parity bit
#-----------------------------------------------------------------------
    def compute_crc8_atm(self, datagram, initial_value=0):
        crc = initial_value
        
        for byte in datagram: # Iterate bytes in data
            for _ in range(0, 8): # Iterate bits in byte
                if (crc >> 7) ^ (byte & 0x01):
                    crc = ((crc << 1) ^ 0x07) & 0xFF
                else:
                    crc = (crc << 1) & 0xFF

                byte = byte >> 1 # Shift to next bit

        return crc
    
#-----------------------------------------------------------------------
# reads the registry on the TMC with a given address.
# returns the binary value of that register
#-----------------------------------------------------------------------
    def read_reg(self, reg):
        self.rFrame[1] = self.mtr_id
        self.rFrame[2] = reg
        self.rFrame[3] = self.compute_crc8_atm(self.rFrame[:-1])
        rt = self.ser.write(bytes(self.rFrame))
        
        if rt != len(self.rFrame):
#             print(f"TMC2209: Err in write", file=sys.stderr)
            return False

        # time.sleep(self.communication_pause)  # adjust per baud and hardware. Sequential reads without some delay fail.
        rtn = self.ser.read() #read what it self
        
        if rtn == None:
#             print("TMC2209: Err in read")
            return ""
        
        print("rtn[7:11] :",rtn[7:11] , rtn[7] ,rtn[8] , rtn[9] , rtn[10] , " , reg : ",reg)
        return rtn[7:11]
#-----------------------------------------------------------------------
# this function tries to read the registry of the TMC 10 times
# if a valid answer is returned, this function returns it as an integer
#-----------------------------------------------------------------------
    def read_int(self, reg):
        tries = 0

        while True:
            rtn = self.read_reg(reg)
            tries += 1

            if len(rtn) >= 4:
                break
            else:
                pass
                print(f"TMC2209: did not get the expected 4 data bytes. Instead got {str(len(rtn))} Bytes")
            
            if tries >= 10:
                print("TMC2209: after 10 tries not valid answer. exiting")
                print("TMC2209: is Stepper Powersupply switched on ?")
                raise SystemExit

        return int.from_bytes(rtn, 'big', False)

#-----------------------------------------------------------------------
# this function can write a value to the register of the tmc
# 1. use read_int to get the current setting of the TMC
# 2. then modify the settings as wished
# 3. write them back to the driver with this function
#-----------------------------------------------------------------------
    def write_reg(self, reg, val):
        #self.ser.reset_output_buffer()
        #self.ser.reset_input_buffer()
        
        self.wFrame[1] = self.mtr_id
        self.wFrame[2] = reg | 0x80; # set write bit
        self.wFrame[3] = 0xFF & (val>>24)
        self.wFrame[4] = 0xFF & (val>>16)
        self.wFrame[5] = 0xFF & (val>>8)
        self.wFrame[6] = 0xFF & val
        self.wFrame[7] = self.compute_crc8_atm(self.wFrame[:-1])
        print("x" ,bytes(self.wFrame),self.wFrame)
 
        rtn = self.ser.write(bytes(self.wFrame))

        if rtn != len(self.wFrame):
#             print(f"TMC2209: Err in write", file=sys.stderr)
            return False

        time.sleep(self.communication_pause)
        return True

#-----------------------------------------------------------------------
# this function als writes a value to the register of the TMC
# but it also checks if the writing process was successfully by checking
# the InterfaceTransmissionCounter before and after writing
#-----------------------------------------------------------------------
    def write_reg_check(self, reg, val):
        ifcnt1 = self.read_int(IFCNT)
        self.write_reg(reg, val)
        ifcnt2 = self.read_int(IFCNT)
        
        if ifcnt1 >= ifcnt2:
            print("TMC2209: writing not successful!")
            print(f"reg: {reg} val: {val}")
            print(f"ifcnt: {ifcnt1} {ifcnt2}")
            return False
        
        return True


#-----------------------------------------------------------------------
# this sets a specific bit to 1
#-----------------------------------------------------------------------
    def set_bit(self, value, bit):
        return value | (bit)

#-----------------------------------------------------------------------
# this sets a specific bit to 0
#-----------------------------------------------------------------------
    def clear_bit(self, value, bit):
        return value & ~(bit)


#-----------------------------------------------------------------------
# TMC_2209
#
# this class has two different functions:
# 1. change setting in the TMC-driver via UART
# 2. move the motor via STEP/DIR pins
#-----------------------------------------------------------------------
class TMC2209:

    tmc_uart = None
    pin_step = 1
    p_pin_step = -1
    p_pin_direction = -1
    p_pin_enable = -1
    debugmode = False
    _micro_stepping_resolution = -1
    _loglevel = Loglevel.info

#-----------------------------------------------------------------------
# constructor
#-----------------------------------------------------------------------
    def __init__(self, 
            rx=setenv.MOTOR_RX_PIN, 
            tx=setenv.MOTOR_TX_PIN, 
            pin_step=setenv.MOTOR_STEP_PIN, 
            pin_direction=setenv.MOTOR_DIR_PIN, 
            pin_enable=setenv.MOTOR_EN_PIN, 
            baudrate=setenv.MOTOR_BAUDRATE, 
            serialport=setenv.MOTOR_NUMBER, 
            ms_resolution=setenv.MOTOR_MICROSTEP):
        self.tmc_uart = TMC_UART(rx, tx, serialport, baudrate)
        self.pin_step = pin_step
        self.p_pin_step = Pin(pin_step, Pin.OUT)
        self.p_pin_direction = Pin(pin_direction, Pin.OUT)
        self.p_pin_enable = Pin(pin_enable, Pin.OUT)
        #self.set_microstepping_resolution(setenv.MOTOR_MICROSTEP)
        #self.clear_general_stat()             
        
#-----------------------------------------------------------------------
# destructor
#-----------------------------------------------------------------------
    def __del__(self):
        if(self.debugmode == True):
            print("TMC2209: Deinit")

        self.set_pin_enable(False)
        self.tmc_uart.deinit()
        
        
#-----------------------------------------------------------------------
# read the register Adress "DRVSTATUS" and prints all current setting
#-----------------------------------------------------------------------
    def driver_status(self):
        print("TMC2209: ---")
        print("TMC2209: DRIVER STATUS:")
        drvstatus = self.tmc_uart.read_int(DRVSTATUS)

        if self.debugmode == True:
            print("TMC2209:", bin(drvstatus))
        
        if drvstatus & stst:
            print("TMC2209: Info: motor is standing still")
        else:
            print("TMC2209: Info: motor is running")

        if drvstatus & stealth:
            print("TMC2209: Info: motor is running on StealthChop")
        else:
            print("TMC2209: Info: motor is running on SpreadCycle")

        cs_act = drvstatus & cs_actual
        cs_act = cs_act >> 16
        print(f"TMC2209: CS actual: {str(cs_act)}")

        if drvstatus & olb:
            print("TMC2209: Warning: Open load detected on phase B")
        
        if drvstatus & ola:
            print("TMC2209: Warning: Open load detected on phase A")
        
        if drvstatus & s2vsb:
            print("TMC2209: Error: Short on low-side MOSFET detected on phase B. The driver becomes disabled")

        if drvstatus & s2vsa:
            print("TMC2209: Error: Short on low-side MOSFET detected on phase A. The driver becomes disabled")

        if drvstatus & s2gb:
            print("TMC2209: Error: Short to GND detected on phase B. The driver becomes disabled. ")
        
        if drvstatus & s2ga:
            print("TMC2209: Error: Short to GND detected on phase A. The driver becomes disabled. ")
        
        if drvstatus & ot:
            print("TMC2209: Error: Driver Overheating!")
        
        if drvstatus & otpw:
            print("TMC2209: Warning: Driver Overheating Prewarning!")
            
#-----------------------------------------------------------------------
# read the register Adress "GCONF" and prints all current setting
#-----------------------------------------------------------------------
    def general_config(self):
        print("TMC2209: ---")
        print("TMC2209: GENERAL CONFIG")
        gconf = self.tmc_uart.read_int(GCONF)

        if self.debugmode == True:
            print("TMC2209:", bin(gconf))

        if gconf & i_scale_analog:
            print("TMC2209: Driver is using voltage supplied to VREF as current reference")
        else:
            print("TMC2209: Driver is using internal reference derived from 5VOUT")
        
        if gconf & internal_rsense:
            print("TMC2209: Internal sense resistors. Use current supplied into VREF as reference.")
            print("TMC2209: VREF pin internally is driven to GND in this mode.")
            print("TMC2209: This will most likely destroy your driver!!!")
            raise SystemExit
        else:
            print("TMC2209: Operation with external sense resistors")
        
        if gconf & en_spreadcycle:
            print("TMC2209: SpreadCycle mode enabled")
        else:
            print("TMC2209: StealthChop PWM mode enabled")
        
        if gconf & shaft:
            print("TMC2209: Inverse motor direction")
        else:
            print("TMC2209: normal motor direction")
        
        if gconf & index_otpw:
            print("TMC2209: INDEX pin outputs overtemperature prewarning flag")
        else:
            print("TMC2209: INDEX shows the first microstep position of sequencer")
        
        if gconf & index_step:
            print("TMC2209: INDEX output shows step pulses from internal pulse generator")
        else:
            print("TMC2209: INDEX output as selected by index_otpw")
        
        if gconf & mstep_reg_select:
            print("TMC2209: Microstep resolution selected by MSTEP register")
        else:
            print("TMC2209: Microstep resolution selected by pins MS1, MS2")
        
#-----------------------------------------------------------------------
# read the register Adress "GSTAT" and prints all current setting
#-----------------------------------------------------------------------
    def general_stat(self):
        print("TMC2209: ---")
        print("TMC2209: GENERAL STAT")
        gstat = self.tmc_uart.read_int(GSTAT)
        
        if self.debugmode == True:
            print("TMC2209:", bin(gstat))
        
        if gstat & reset:
            print("TMC2209: The Driver has been reset since the last read access to GSTAT")
        
        if gstat & drv_err:
            print("TMC2209: The driver has been shut down due to overtemperature or short circuit detection since the last read access")
        
        if gstat & uv_cp:
            print("TMC2209: Undervoltage on the charge pump. The driver is disabled in this case")
        
#-----------------------------------------------------------------------
# read the register Adress "GSTAT" and prints all current setting
#-----------------------------------------------------------------------
    def clear_general_stat(self):        
        gstat = self.tmc_uart.read_int(GSTAT)
        gstat = self.tmc_uart.set_bit(gstat, reset)
        gstat = self.tmc_uart.set_bit(gstat, drv_err)
        self.tmc_uart.write_reg_check(GSTAT, gstat)

#-----------------------------------------------------------------------
# read the register Adress "IOIN" and prints all current setting
#-----------------------------------------------------------------------
    def ioin(self):
        print("TMC2209: ---")
        print("TMC2209: INPUTS")
        ioin = self.tmc_uart.read_int(IOIN)
        
        if self.debugmode == True:
            print("TMC2209:", bin(ioin))
        
        if ioin & io_spread:
            print("TMC2209: spread is high")
        else:
            print("TMC2209: spread is low")

        if ioin & io_dir:
            print("TMC2209: dir is high")
        else:
            print("TMC2209: dir is low")

        if ioin & io_step:
            print("TMC2209: step is high")
        else:
            print("TMC2209: step is low")

        if ioin & io_enn:
            print("TMC2209: en is high")
        else:
            print("TMC2209: en is low")
        
#-----------------------------------------------------------------------
# read the register Adress "CHOPCONF" and prints all current setting
#-----------------------------------------------------------------------
    def chopper_control(self):
        print("TMC2209: ---")
        print("TMC2209: CHOPPER CONTROL")
        chopconf = self.tmc_uart.read_int(CHOPCONF)

        if self.debugmode == True:
            print("TMC2209:", bin(chopconf))
        
        print(f"TMC2209: native {str(self.get_microstepping_resolution())} microstep setting")
        
        if chopconf & intpol:
            print("TMC2209: interpolation to 256 microsteps")
        
        if chopconf & vsense:
            print("TMC2209: 1: High sensitivity, low sense resistor voltage")
        else:
            print("TMC2209: 0: Low sensitivity, high sense resistor voltage")

#-----------------------------------------------------------------------
# enables or disables the motor current output
#-----------------------------------------------------------------------
    def set_pin_enable(self, enabled):
        if enabled:
            self.p_pin_enable.off()
#             print(f"TMC2209: Motor output active = {enabled}")
        else:
            self.p_pin_enable.on()
        # print("en",self.p_pin_enable.value(),enabled)
#         print(f"TMC2209: Motor output active = {enabled}")

#-----------------------------------------------------------------------
# sets the motor shaft direction to the given value: 0 = CCW; 1 = CW
#-----------------------------------------------------------------------
    def set_pin_direction(self, direction):
        if (self.p_pin_direction.value() != direction):
#             print("change",self.p_pin_direction.value() , direction)
      
            if direction:
                self.p_pin_direction.on()
            else:
                self.p_pin_direction.off()
#             print("value",self.p_pin_direction.value() )
            

#-----------------------------------------------------------------------
# enables or disables the motor step output
#-----------------------------------------------------------------------
    def set_pin_step(self, enabled):
        if enabled:
            self.p_pin_step.on()
        else:
            self.p_pin_step.off()

#-----------------------------------------------------------------------
# returns the motor shaft direction: 0 = CCW; 1 = CW
#-----------------------------------------------------------------------
    def get_direction_shart(self):
        
        return self.tmc_uart.read_int(GCONF) & shaft
    

#-----------------------------------------------------------------------
# sets the motor shaft direction to the given value: 0 = CCW; 1 = CW
#-----------------------------------------------------------------------
    def set_direction_shart(self, direction):        
        gconf = self.tmc_uart.read_int(GCONF)
        if (self.get_direction_shart()/8!=direction):

            if direction:
                if self.debugmode == True:
                    print("TMC2209: write inverse motor direction")

                gconf = self.tmc_uart.set_bit(gconf, shaft)
            else:
                if self.debugmode == True:
                    print("TMC2209: write normal motor direction")

                gconf = self.tmc_uart.clear_bit(gconf, shaft)
#             print(direction)
            self.tmc_uart.write_reg_check(GCONF, gconf)
            time.sleep_us(100)
  
#-----------------------------------------------------------------------
# return whether the tmc inbuilt interpolation is active
#-----------------------------------------------------------------------
    def get_interpolation(self):
        return self.tmc_uart.read_int(CHOPCONF) & intpol

#-----------------------------------------------------------------------
# enables the tmc inbuilt interpolation of the steps to 256 microsteps
#-----------------------------------------------------------------------
    def set_interpolation(self, enabled):
        chopconf = self.tmc_uart.read_int(CHOPCONF)

        if enabled:
            chopconf = self.tmc_uart.set_bit(chopconf, intpol)
        else:
            chopconf = self.tmc_uart.clear_bit(chopconf, intpol)

        if self.debugmode == True:
            print(f"TMC2209: writing microstep interpolation setting =", enabled)

        self.tmc_uart.write_reg_check(CHOPCONF, chopconf)

#-----------------------------------------------------------------------
# return whether spreadcycle (1) is active or stealthchop (0)
#-----------------------------------------------------------------------
    def get_spread_cycle(self):
        return self.tmc_uart.read_int(GCONF) & en_spreadcycle

#-----------------------------------------------------------------------
# enables spreadcycle (1) or stealthchop (0)
#-----------------------------------------------------------------------
    def set_spread_cycle(self, enabled):
        gconf = self.tmc_uart.read_int(GCONF)

        if enabled:
            if self.debugmode == True:
                print("TMC2209: Spreadcycle = activated")
            
            gconf = self.tmc_uart.set_bit(gconf, en_spreadcycle)
        else:
            if self.debugmode == True:
                print("TMC2209: Stealthchop = deactivated")
            
            gconf = self.tmc_uart.clear_bit(gconf, en_spreadcycle)
        
        self.tmc_uart.write_reg_check(GCONF, gconf)

#-----------------------------------------------------------------------
# returns the current native microstep resolution (1-256)
#-----------------------------------------------------------------------
    def get_microstepping_resolution(self):
        chopconf = self.tmc_uart.read_int(CHOPCONF)
        print("chopconf  : ", chopconf)
        msresdezimal = chopconf & (msres0 | msres1 | msres2 | msres3)
        msresdezimal = msresdezimal >> 24
        msresdezimal = 8 - msresdezimal
        self._micro_stepping_resolution = int(math.pow(2, msresdezimal))

        return self._micro_stepping_resolution

#-----------------------------------------------------------------------
# sets the current native microstep resolution (1,2,4,8,16,32,64,128,256)
#-----------------------------------------------------------------------
    def set_microstepping_resolution(self, msres):      
        chopconf = self.tmc_uart.read_int(CHOPCONF)
        chopconf = chopconf & (~msres0 | ~msres1 | ~msres2 | ~msres3) #setting all bits to zero
        print("chopconf 1 :",chopconf)

        msresdezimal = int(math.log(msres, 2))
        msresdezimal = 8 - msresdezimal
        print("msresdezimal 2 :",msresdezimal)

        chopconf = int(chopconf) & int(4043309055)
        print("chopconf 2 :",chopconf)

        chopconf = chopconf | msresdezimal << 24
        print("chopconf 3 :",chopconf)

        if self.debugmode == True:
            print(f"TMC2209: writing {str(msres)} microstep setting")

        self.tmc_uart.write_reg_check(CHOPCONF, chopconf)
        self.set_microstep_resolution_regselect(True)


#-----------------------------------------------------------------------
# sets the register bit "mstep_reg_select" to 1 or 0 depending to the given value.
# this is needed to set the microstep resolution via UART
# this method is called by "setMicrosteppingResolution"
#-----------------------------------------------------------------------
    def set_microstep_resolution_regselect(self, enabled):                  
        gconf = self.tmc_uart.read_int(GCONF)
        print("gconf1 :",gconf)

        if enabled:
            gconf = self.tmc_uart.set_bit(gconf, mstep_reg_select)
        else:
            gconf = self.tmc_uart.clear_bit(gconf, mstep_reg_select)

        if self.debugmode == True:
            print(f"TMC2209: writing MStep Reg Select = {enabled}")
        print("gconf2 :",gconf)

        self.tmc_uart.write_reg_check(GCONF, gconf)

#-----------------------------------------------------------------------
# returns how many steps are needed for one revolution
#-----------------------------------------------------------------------
    def get_steps_per_resolution(self):
        return 200 * self.get_microstepping_resolution()

#-----------------------------------------------------------------------
# returns the current Microstep counter.
# Indicates actual position in the microstep table for CUR_A
#-----------------------------------------------------------------------
    def get_microstep_counter(self, in_steps = False, offset = 0):
        mscnt = self.tmc_uart.read_int(MSCNT)
        
        if not in_steps:
            return mscnt

        return round((4 * self._micro_stepping_resolution) - ((mscnt - 64) * (self._micro_stepping_resolution * 4) / 1024) - 1) + offset

#-----------------------------------------------------------------------
# returns how many steps are needed for one rotation
#-----------------------------------------------------------------------
    def get_steps_per_rotation(self):
        return self.get_steps_per_resolution() * 21

#-----------------------------------------------------------------------
# return the current stallguard result
# its will be calculated with every fullstep
# higher values means a lower motor load
#-----------------------------------------------------------------------
    def get_tstep(self):
        return self.tmc_uart.read_int(TSTEP)
ehyfbzeyfeunz,ujefz
#-----------------------------------------------------------------------
# sets the register bit "VACTUAL" to to a given value
# VACTUAL allows moving the motor by UART control.
# It gives the motor velocity in +-(2^23)-1 [μsteps / t]
# 0: Normal operation. Driver reacts to STEP input
#-----------------------------------------------------------------------
    def set_vactual(self, value):
        self.tmc_uart.write_reg_check(VACTUAL, int(round(value)))

#-----------------------------------------------------------------------
# return the current stallguard result
# its will be calculated with every fullstep
# higher values means a lower motor load
#-----------------------------------------------------------------------
    def get_stallguard(self):
        return self.tmc_uart.read_int(SG_RESULT)

#-----------------------------------------------------------------------
# sets the register bit "SGTHRS" to to a given value
# this is needed for the stallguard interrupt callback
# SG_RESULT becomes compared to the double of this threshold.
# SG_RESULT ≤ SGTHRS*2
#-----------------------------------------------------------------------
    def set_stallguard_threshold(self, threshold):
        if self.debugmode == True:
            print("TMC2209: sgthrs =", bin(threshold))

        self.tmc_uart.write_reg_check(SGTHRS, threshold)

#-----------------------------------------------------------------------
# This  is  the  lower  threshold  velocity  for  switching  
# on  smart energy CoolStep and StallGuard to DIAG output. (unsigned)
#-----------------------------------------------------------------------
    def set_coolstep_threshold(self, threshold):
        if self.debugmode == True:
            print("TMC2209: tcoolthrs =", bin(threshold))

        self.tmc_uart.write_reg_check(TCOOLTHRS, threshold)

#-----------------------------------------------------------------------
# set a function to call back, when the driver detects a stall 
# via stallguard
# high value on the diag pin can also mean a driver error
#-----------------------------------------------------------------------
    def set_stallguard_callback(self, stallguard_pin, threshold, callback, min_speed = 2000):
        self.set_stallguard_threshold(threshold)
        self.set_coolstep_threshold(min_speed)

        if self.debugmode == True:
            print("TMC2209: setup stallguard callback")
        
        p25 = Pin(stallguard_pin, Pin.IN, Pin.PULL_DOWN)
        p25.irq(trigger=Pin.IRQ_RISING, handler=callback)


#-----------------------------------------------------------------------
# return whether Vref (1) or 5V (0) is used for current scale
#-----------------------------------------------------------------------
    def get_iscale_analog(self):
        return self.tmc_uart.read_int(GCONF) & i_scale_analog

#-----------------------------------------------------------------------
# sets Vref (1) or 5V (0) for current scale
#-----------------------------------------------------------------------
    def set_iscale_analog(self, enabled):        
        gconf = self.tmc_uart.read_int(GCONF)

        if enabled:
            if self.debugmode == True:
                print("TMC2209: activated Vref for current scale")

            gconf = self.tmc_uart.set_bit(gconf, i_scale_analog)
        else:
            if self.debugmode == True:
                print("TMC2209: activated 5V-out for current scale")

            gconf = self.tmc_uart.clear_bit(gconf, i_scale_analog)

        self.tmc_uart.write_reg_check(GCONF, gconf)

#-----------------------------------------------------------------------
# returns which sense resistor voltage is used for current scaling
# 0: Low sensitivity, high sense resistor voltage
# 1: High sensitivity, low sense resistor voltage
#-----------------------------------------------------------------------
    def get_voltage_sense(self):
        return self.tmc_uart.read_int(CHOPCONF) & vsense

#-----------------------------------------------------------------------
# sets which sense resistor voltage is used for current scaling
# 0: Low sensitivity, high sense resistor voltage
# 1: High sensitivity, low sense resistor voltage
#-----------------------------------------------------------------------
    def set_voltage_sense(self, enabled):      
        chopconf = self.tmc_uart.read_int(CHOPCONF)

        if enabled:
            if self.debugmode == True:
                print("TMC2209: activated High sensitivity, low sense resistor voltage")

            chopconf = self.tmc_uart.set_bit(chopconf, vsense)
        else:
            if self.debugmode == True:
                print("TMC2209: activated Low sensitivity, high sense resistor voltage")
            
            chopconf = self.tmc_uart.clear_bit(chopconf, vsense)
        
        self.tmc_uart.write_reg_check(CHOPCONF, chopconf)

#-----------------------------------------------------------------------
# returns which sense resistor voltage is used for current scaling
# 0: Low sensitivity, high sense resistor voltage
# 1: High sensitivity, low sense resistor voltage
#-----------------------------------------------------------------------
    def get_internal_resistor_sense(self):
        return self.tmc_uart.read_int(GCONF) & internal_rsense

#-----------------------------------------------------------------------
# sets which sense resistor voltage is used for current scaling
# 0: Low sensitivity, high sense resistor voltage
# 1: High sensitivity, low sense resistor voltage
#-----------------------------------------------------------------------
    def set_internal_resistor_sense(self, enabled):        
        gconf = self.tmc_uart.read_int(GCONF)

        if enabled:
            if self.debugmode == True:
                print("TMC2209: activated internal sense resistors.")

            gconf = self.tmc_uart.set_bit(gconf, internal_rsense)
        else:
            if self.debugmode == True:
                print("TMC2209: activated operation with external sense resistors")

            gconf = self.tmc_uart.clear_bit(gconf, internal_rsense)

        self.tmc_uart.write_reg_check(GCONF, gconf)

#-----------------------------------------------------------------------
# sets the current scale (CS) for Running and Holding
# and the delay, when to be switched to Holding current
# IHold = 0-31; IRun = 0-31; IHoldDelay = 0-15
#-----------------------------------------------------------------------
    def set_irun_ihold(self, IHold, IRun, IHoldDelay):
        ihold_irun = 0
        ihold_irun = ihold_irun | IHold << 0
        ihold_irun = ihold_irun | IRun << 8
        ihold_irun = ihold_irun | IHoldDelay << 16

        if self.debugmode == True:
            print("TMC2209: ihold_irun =", bin(ihold_irun))

        self.tmc_uart.write_reg_check(IHOLD_IRUN, ihold_irun)

#-----------------------------------------------------------------------
# sets the current flow for the motor
# run_current in mA
# check whether Vref is actually 1.2V
#-----------------------------------------------------------------------
    def set_current_flow(self, run_current, hold_current_multiplier = 0.5, hold_current_delay = 10, Vref = setenv.MOTOR_VREF):
        CS_IRun = 0
        Rsense = 0.11
        Vfs = 0

        if self.get_voltage_sense():
            if self.debugmode == True:
                print("TMC2209: Vsense: 1")

            Vfs = 0.180 * Vref / 2.5
        else:
            if self.debugmode == True:
                print("TMC2209: Vsense: 0")

            Vfs = 0.325 * Vref / 2.5
            
        CS_IRun = 32.0 * 1.41421 * run_current / 1000.0 * (Rsense + 0.02) / Vfs - 1
        CS_IRun = min(CS_IRun, 31)
        CS_IRun = max(CS_IRun, 0)
        CS_IHold = hold_current_multiplier * CS_IRun
        CS_IRun = round(CS_IRun)
        CS_IHold = round(CS_IHold)
        hold_current_delay = round(hold_current_delay)

        if self.debugmode == True:
            print("TMC2209: CS_IRun =", CS_IRun)
            print("TMC2209: CS_IHold =", CS_IHold)
            print("TMC2209: Delay =", hold_current_delay)

        self.set_irun_ihold(CS_IHold, CS_IRun, hold_current_delay)
        
#-----------------------------------------------------------------------
# tests the EN, DIR and STEP pin
# this sets the EN, DIR and STEP pin to HIGH, LOW and HIGH
# and checks the IOIN Register of the TMC meanwhile
#-----------------------------------------------------------------------
    def test_pins(self):
        pin_dir_ok = pin_step_ok = pin_en_ok = True
        
        self.p_pin_step.on()
        self.p_pin_direction.on()
        self.p_pin_enable.on()

        time.sleep(0.1)
        ioin = self.tmc_uart.read_int(IOIN)
        
        if not(ioin & io_dir):
            pin_dir_ok = False
        
        if not(ioin & io_step):
            pin_step_ok = False
        
        if not(ioin & io_enn):
            pin_en_ok = False

        self.p_pin_step.off()
        self.p_pin_direction.off()
        self.p_pin_enable.off()
        
        time.sleep(0.1)
        ioin = self.tmc_uart.read_int(IOIN)
        
        if ioin & io_dir:
            pin_dir_ok = False
        
        if ioin & io_step:
            pin_step_ok = False
        
        if ioin & io_enn:
            pin_en_ok = False

        self.p_pin_step.on()
        self.p_pin_direction.on()
        self.p_pin_enable.on()
        
        time.sleep(0.1)
        ioin = self.tmc_uart.read_int(IOIN)
        
        if not(ioin & io_dir):
            pin_dir_ok = False
            
        if not(ioin & io_step):
            pin_step_ok = False
            
        if not(ioin & io_enn):
            pin_en_ok = False

        self.set_pin_enable(False)

        print("---")
        print(f"Pin STEP: \t{pin_step_ok}")
        print(f"Pin DIRECTION: \t{pin_dir_ok}")
        print(f"Pin ENABLE: \t{pin_en_ok}")
        print("---")
