from machine import Pin, PWM, SPI
import time
import utime
from config import *
from led import *
from sTimer import sTimer_Start, sTimer_Stop
import sys

EN = Pin(13, Pin.OUT)
DIR = Pin(14, Pin.OUT)
STEP = Pin(15, Pin.OUT)
ENDSTOP = Pin(8, Pin.IN, Pin.PULL_UP)
DC_EN = Pin(11, Pin.OUT)
DC_IN = Pin(10, Pin.OUT)
DC_OUT = Pin(12, Pin.IN)
SPI_MODE = Pin(20, Pin.OUT)
spi_cs = Pin(17, Pin.OUT)

spi = SPI(0, baudrate=100000, polarity=1, phase=1, bits=8, firstbit=SPI.MSB, sck=Pin(18, Pin.OUT), mosi=Pin(19, Pin.OUT), miso=Pin(16, Pin.IN))

DC_EN(0)
DC_IN(0)
spi_cs(1)
EN(1) #0 = enabled
DIR(0)
STEP(0)

MM_PER_STEP = SCREW_PITCH / (MOTOR_STEPS * MICROSTEPPING)
FREQ_PER_STEP_INCREASE = (((MOTOR_STEPS * MICROSTEPPING) / SCREW_PITCH) * ACCELERATION) / 60

REG_GCONF = 0x00
REG_GSTAT = 0x01
REG_IHOLD_IRUN = 0x10
REG_CHOPCONF = 0x6C
REG_COOLCONF = 0x6D
REG_DCCTRL = 0x6E
REG_DRVSTATUS = 0x6F

step_cntr = 0
target_steps = 0
target_frequency = 0
current_frequency = 0
def step():
    global STEP, step_cntr, current_frequency, target_frequency, target_steps, FREQ_PER_STEP_INCREASE
    STEP.value(not STEP.value())
    STEP.value(not STEP.value())
    LED_ERR.value(not LED_ERR.value())
    
    step_cntr += 1
    if(current_frequency < target_frequency):
        current_frequency += FREQ_PER_STEP_INCREASE
    if(target_steps >= step_cntr):
        pass
 
def rotate_motor(steps, feedrate):
    global current_frequency, step_cntr, target_steps, target_frequency
    step_cntr = 0
    target_steps = steps
    target_frequency = ((feedrate / SCREW_PITCH) * MOTOR_STEPS * MICROSTEPPING) / 60 #steps/s = rpm * steps_per_rotation / 60
    print(target_frequency)
    print(FREQ_PER_STEP_INCREASE)
    current_frequency = FREQ_PER_STEP_INCREASE

def timer_callback(counter):
    if (counter % 1) == 0:
        step()

def get_stats():
    global STEP, step_cntr, current_frequency, target_frequency, target_steps, FREQ_PER_STEP_INCREASE
    print(step_cntr, current_frequency, target_frequency, FREQ_PER_STEP_INCREASE)
    
def rotate_motor_test():
    rotate_motor(32*200, 5)
    sTimer_Start()
    time.sleep(2)
    sTimer_Stop()

def prepare_spi_msg(addr, d0, d1, d2, d3, rw):
    msg = bytearray()
    if rw == 'r':
        msg.append(addr)
    else:
        msg.append(0x80 + addr)
    msg.append(d0)
    msg.append(d1)
    msg.append(d2)
    msg.append(d3)
    
    return msg

def send_spi_msg(msg):
    spi_cs(0)
    rxdata = bytearray(5)
    spi.write_readinto(msg, rxdata)
    #print(rxdata.hex())
    spi_cs(1)
    time.sleep(0.01)
    
def TMC2130_Rotate(dir, steps, speed): #speed in rpm
    #1rmp = 200steps/min=3.333steps/s~150ms@50%duty    
    wait = (((1 / ((MOTOR_STEPS * MICROSTEPPING) / 60)) / 2) / speed) * 1000 * 1000
    freq = 1 / ((wait / 1000000) * 2)

    if AXIS_REVERSE:
        if dir == 0:
            dir = 1
        else:
            dir = 0

    DIR(dir)
    LED_DIR(dir)
    
    STEP = PWM(Pin(15), freq=int(freq), duty_u16=32768)
    time.sleep((1 / freq) * steps)
    STEP.deinit()

homing = False
def TMC2130_Home():
    global homing
    
    dir = HOME_DIR
    STEP = Pin(15, Pin.OUT)
    
    steps_mm = (MOTOR_STEPS * MICROSTEPPING) / SCREW_PITCH
    rpm = HOME_SPEED / SCREW_PITCH
    wait = (((1 / ((MOTOR_STEPS * MICROSTEPPING) / 60)) / 2) / rpm) * 1000 * 1000
    freq = 1 / ((wait / 1000000) * 2)
    
    if AXIS_REVERSE:
        if dir == 0:
            dir = 1
        else:
            dir = 0
            
    DIR(dir)
    LED_DIR(dir)

    sys.stdout.write('homing\r\n')
    homing = True
    STEP = PWM(Pin(15), freq=int(freq), duty_u16=32768)
    while homing:
        pass
    STEP.deinit()
    
    if HOME_DEBOUNCE == True:
        sys.stdout.write('debouncing\r\n')
        if dir == 0:
            dir = 1
        else:
            dir = 0
            
        DIR(dir)
        LED_DIR(dir)
        
        steps = steps_mm * abs(HOME_DEBOUNCE_DISTANCE)
        
        TMC2130_Rotate(dir, steps, HOME_DEBOUNCE_SPEED)
        
        rpm = HOME_DEBOUNCE_SPEED / SCREW_PITCH
        wait = (((1 / ((MOTOR_STEPS * MICROSTEPPING) / 60)) / 2) / rpm) * 1000 * 1000
        freq = 1 / ((wait / 1000000) * 2)
        
        if dir == 0:
            dir = 1
        else:
            dir = 0
            
        DIR(dir)
        LED_DIR(dir)
        
        homing = True
        STEP = PWM(Pin(15), freq=int(freq), duty_u16=32768)
        while homing:
            pass
        STEP.deinit()
    
def TMC2130_Disable():
    EN.value(1)
    
def TMC2130_Enable():
    EN.value(0)

def endstop_callback(endstop):
    global homing
    
    homing = False

def TMC2130_Init():
    SPI_MODE(1)
    # setup coolstep
    microstep = 0
    if MICROSTEPPING == 128:
        microstep = 1
    elif MICROSTEPPING == 64:
        microstep = 2
    elif MICROSTEPPING == 32:
        microstep = 3
    elif MICROSTEPPING == 16:
        microstep = 4
    elif MICROSTEPPING == 8:
        microstep = 5
    elif MICROSTEPPING == 4:
        microstep = 6
    elif MICROSTEPPING == 2:
        microstep = 7
    elif MICROSTEPPING == 1:
        microstep = 8
    else:
        microstep = 0
    
    msg = prepare_spi_msg(REG_CHOPCONF, 0x00+microstep, 0x01, 0x00, 0xC3, 'w')
    send_spi_msg(msg)
    msg = prepare_spi_msg(REG_IHOLD_IRUN, 0x00, 0x06, I_RUN, I_HOLD, 'w')
    send_spi_msg(msg)
    msg = prepare_spi_msg(0x11, 0x00, 0x00, 0x00, 0x0A, 'w')
    send_spi_msg(msg)
    msg = prepare_spi_msg(REG_GCONF, 0x00, 0x00, 0x00, 0x04, 'w')
    send_spi_msg(msg)
    msg = prepare_spi_msg(0x13, 0x00, 0x00, 0x01, 0xF4, 'w')
    send_spi_msg(msg)
    msg = prepare_spi_msg(0x70, 0x00, 0x04, 0x01, 0xC8, 'w')
    send_spi_msg(msg)
    msg = prepare_spi_msg(REG_COOLCONF, 0x00, 0x00, 0x03, 0x01, 'w')
    send_spi_msg(msg)
    
    TMC2130_Enable()
    
    ENDSTOP.irq(trigger=Pin.IRQ_FALLING, handler=endstop_callback)