import time
from as5600l import AS5600L
from tmc_2209 import TMC2209
import setenv
from machine import Pin
import gc
#-----------------------------------------------------------------------
# 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
#-----------------------------------------------------------------------
class SmartMotor(TMC2209):
    status = False

    position = 0
    if setenv.MOTOR_SENSOR_END_PIN != -1 :
        init_home_end =setenv.MOTOR_SENSOR_END_VALUE
        pin_home_end = Pin(setenv.MOTOR_SENSOR_END_PIN, Pin.IN)
    if setenv.MOTOR_SENSOR_BEGIN_PIN != -1 :
        init_home_begin =setenv.MOTOR_SENSOR_BEGIN_VALUE
        pin_home_begin = Pin(setenv.MOTOR_SENSOR_BEGIN_PIN, Pin.IN)

    def get_encoder_position(self,encoder):
        if self.encoder is not None:
            self.actual = self.encoder.get_angle_degrees()
            if self.actual != None and self.actual != self.last:
                self.calc = self.actual-self.last
                if self.actual < self.last :
                    if self.calc<-3800:
                        self.calc = abs((4096 - self.last + self.actual))
                    self.posreal+=self.calc
                elif self.actual > self.last :
                    if self.calc>3800:
                        self.calc = abs((4096 - self.actual + self.last))
                    self.posreal+=self.calc
                self.last = self.actual
            return(self.posreal)

    def init_encoder(self):
        self.encoder = AS5600L()
        self.actual = 0
        self.last = 0
        print(self.encoder.get_status())
        self.posbegin = self.encoder.get_angle_degrees_fast()
        print("posbegin",self.posbegin)
        self.posreal = 0
        self.last=self.posbegin

    def reset_position(self):
        self.position = 0
    
    # def set_home_pins(self, start = -1, end = -1):

    def set_encoder(self, encoder):
        self.encoder = encoder

    def init_vactual(self):
        self.clear_general_stat()
        time.sleep_us(10)        
        self.set_voltage_sense(False)#ok
        self.set_current_flow(setenv.MOTOR_AMP)
        self.set_iscale_analog(False)#ok
        self.set_interpolation(True)
        self.set_internal_resistor_sense(False)
        self.set_spread_cycle(True)
        self.set_microstepping_resolution(setenv.MOTOR_MICROSTEP)
        self.set_microstep_resolution_regselect(True)
        self.set_pin_enable(True)
        return True
     
    def rotate(self, 
            vactual, 
            degrees = 0, 
            acceleration = 0,
            deceleration = 0,
            KP = 1.0, 
            KI = 0,
            KD = 0,
            counter = False,
            show_stallguard_result = False, 
            show_tstep = False,
            absrel = True):
        self.init_vactual()
        liste_vitesse = [0]
        liste_time = [0]
        liste_distance = [0]   
        if(absrel):
            if degrees - self.position  < 0:
                print(degrees - self.position)
                self.set_direction_shart(False)
                degrees = abs(degrees - self.position)
                sense = False
            else:
                degrees = degrees - self.position
                sense = True
                self.set_direction_shart(True)

        vactual = int(round((vactual/0.715)*self.get_steps_per_resolution())) # not tested!
        self._stop = False
        current_vactual = 0        
        last_error = 0
        integral = 0

    
        if self.debugmode == True:
            if(degrees != 0):
                print("vactual: "+str(vactual)+" for "+str(degrees)+" degrees")
        

        if (degrees != 0):
            start_time = time.ticks_ms()
            end_time = start_time
            current_degrees = 0
            timespan = 0
            error= 0
            self.set_pin_enable(True)
            while((current_degrees <= degrees)):
                print(current_degrees)
                error = degrees - current_degrees
                integral = integral + error
                derivative = error - last_error
                speed_adjustment = KP * error + KI * integral + KD * derivative
                last_error = error
                
                start_time= time.ticks_ms()
                gap_time = (start_time-end_time)/1000
                rps = current_vactual/self.get_steps_per_resolution()*0.715
                angle = rps*gap_time*360
                current_degrees = current_degrees+angle
                timespan +=gap_time
                if(sense):
                    self.position += angle
                else:
                    self.position -= angle
                if ((current_vactual==0) or (error/current_vactual > current_vactual/deceleration)):
                    if(acceleration != 0 and abs(current_vactual)<abs(vactual)):
                        current_vactual += acceleration*gap_time
                        if current_vactual>vactual:
                            current_vactual=vactual
                else:
                    current_vactual -= deceleration*gap_time
#                 liste_vitesse.append(current_vactual*setenv.MOTOR_MICROSTEP)
#                 liste_time.append(liste_time[len(liste_time)-1] + gap_time)
#                 liste_distance.append(current_degrees)
                print(current_vactual*setenv.MOTOR_MICROSTEP)
                self.set_vactual(current_vactual*setenv.MOTOR_MICROSTEP)
                gc.collect()
                end_time=start_time
        self.set_pin_enable(False)            
        self.set_vactual(0)
#         print("============vitesse==========")
#         print(liste_vitesse)
#         print("============temps============")
#         print(liste_time)
#         print("============distance============")
#         print(liste_distance)
#         print("=============================")

        return ('fin de position a : ' + str(self.position) + ' degres')

    def rotate_Closed_Loop(self, 
            vactual, 
            degrees = 0, 
            acceleration = 0,
            deceleration = 0,
            KP = 1.0, 
            KI = 0,
            KD = 0,
            counter = False,
            show_stallguard_result = False, 
            show_tstep = False,
            absrel = True):
        self.init_vactual()
        print(self.get_encoder_position(self.encoder))
        if(absrel):
            print(self.get_encoder_position(self.encoder))
            if (degrees - self.get_encoder_position(self.encoder))  < 0:
                self.set_direction_shart(False)
                sense = False
            else:
                self.set_direction_shart(True)
                sense = True

        vactual = int(round((vactual/0.715)*self.get_steps_per_resolution())) # not tested!
        self._stop = False
        current_vactual = 0        
        last_error = 0
        integral = 0

        if (vactual<0):
            acceleration = -acceleration
    
        if self.debugmode == True:
            if(degrees != 0):
                print("vactual: "+str(vactual)+" for "+str(degrees)+" degrees")

        
        if (degrees != 0):
            current_degrees = self.get_encoder_position(0)
            self.set_pin_enable(True)
            start_time = time.ticks_ms()
            end_time = start_time
            current_degrees = 0
            timespan = 0

            if(degrees - current_degrees  < 0):
                self.set_set_direction_shart_shart(False)
                while((not self._stop) and (current_degrees <= degrees)):
                    current_degrees = self.get_encoder_position(0)

                    error = degrees - current_degrees
                    integral = integral + error
                    derivative = error - last_error
                    speed_adjustment = KP * error + KI * integral + KD * derivative
                    last_error = error

                    start_time= time.ticks_ms()
                    gap_time = (start_time-end_time)/1000
                    rps = current_vactual/self.get_steps_per_resolution()*0.715
                    angle = rps*gap_time*360
                    current_degrees = current_degrees+angle
                    timespan +=gap_time
                    if(sense):
                        self.position += angle
                    else:
                        self.position -= angle

                    if ((current_vactual==0) or (error/current_vactual > current_vactual/deceleration)):
                        if(acceleration != 0 and abs(current_vactual)<abs(vactual)):
                            current_vactual += acceleration*gap_time
                            if current_vactual>vactual:
                                current_vactual=vactual
                            self.set_vactual(current_vactual)
                    else:
                        current_vactual -= deceleration*gap_time
                        self.set_vactual(current_vactual)
        self.set_pin_enable(False)            
        self.set_vactual(0)
        return ('fin de position a : ' + str(self.position) + ' degres')
    
    def stop(self):
        self.set_vactual(0)
        time.sleep(1)
        self.set_pin_enable(False)
        self.p_pin_enable.off()
#-----------------------------------------------------------------------
# This function is for doing Home
# Setup "mode" to 'begin' for Home 0 and 'end' for go to max range
#For uses absolute position
#-----------------------------------------------------------------------   
    
    def go_home(self,mode=False):
        self.run_vactual()
        if(mode!=False):
            if(mode == "begin"):
                current_vactual = 0  
                if self.pin_home_begin != -1:
                    self.set_pin_enable(True)
                    self.set_direction_shart(False)
                    vactual = int(round((1/0.715)*self.get_steps_per_resolution()))
                    while self.pin_home_begin.value() == self.init_home_begin:
                        if(abs(current_vactual)<abs(vactual)):
                            current_vactual += 50*0.05
                            if current_vactual>vactual:
                                current_vactual=vactual
                            self.set_vactual(current_vactual)
                    self.set_vactual(0)
                    time.sleep(0.2)
                    self.set_direction_shart(True)
                    current_vactual = 0                      
                    while self.pin_home_begin.value() != self.init_home_begin:
                        if(abs(current_vactual)<abs(vactual)):
                            current_vactual += 50*0.05
                            if current_vactual>vactual:
                                current_vactual=vactual
                            self.set_vactual(current_vactual)
                else :
                    print("Any begin sensor setup")
                    return False
            if(mode == "end"):
                if self.pin_home_end != -1:
                    self.set_pin_enable(True)
                    self.set_direction_shart(True)
                    vactual = int(round((1.5/0.715)*self.get_steps_per_resolution()))
                    while self.pin_home_end.value() == self.init_home_end:
                        if(abs(current_vactual)<abs(vactual)):
                            current_vactual += 100*0.05
                            if current_vactual>vactual:
                                current_vactual=vactual
                            self.set_vactual(current_vactual)
                    self.set_vactual(0)
                    time.sleep(0.2)
                    self.set_direction_shart(False)
                    current_vactual = 0  
                    while self.pin_home_end.value() != self.init_home_end:
                        if(abs(current_vactual)<abs(vactual)):
                            current_vactual += 100*0.05
                            if current_vactual>vactual:
                                current_vactual=vactual
                            self.set_vactual(int(round(current_vactual)))
                else :
                    print("Any end sensor setup")
                    return False

        else:
            self.set_voltage_sense(True)
            self.set_current_flow(1800)
            self.set_iscale_analog(True)
            self.set_interpolation(True)
            self.set_spread_cycle(True)
            self.set_microstepping_resolution(setenv.MOTOR_MICROSTEP)
            self.set_internal_resistor_sense(False)
            self.set_microstep_resolution_regselect(True)
            print(self.rotation(500,degrees=-self.position,acceleration=1000,deceleration=1000))
        self.set_vactual(0)
        self.position = 0
        return True
        
#-----------------------------------------------------------------------
# return the current position result
#-----------------------------------------------------------------------   
    
    def get_position_degrees(self):
        return self.position
