from machine import Pin
import time
import uasyncio as asyncio

stepping = [[1,0,1,0],[1,0,0,0],[1,0,0,1],[0,0,0,1],[0,1,0,1],[0,1,0,0],[0,1,1,0],[0,0,1,0]]

class Stepper:
    def __init__(self,pins,stepsPerRev=400,stepDurationMs=10,inverted=False):
        self._pins = [Pin(p, Pin.OUT) for p in  pins]
        self.reset()
        self.stepDurationMs = stepDurationMs
        self._speedMs = 0
        self._lastStepTime = time.time()
        self.stepsPerRev=stepsPerRev
        self.inverted = inverted
        loop = asyncio.get_event_loop()
        loop.create_task(self._update_async())

    def reset(self):
        self.stepnum = 0
        self._rotTarget = 0
        self.rotDirection = 0

    def _setPins(self,level):
        self._pins[0].value(level[0])
        self._pins[1].value(level[1])
        self._pins[2].value(level[2])
        self._pins[3].value(level[3])

    def getStepsToTarget(self):
        #return ((self._rotTarget - self.stepnum)*self.rotDirection)%self.stepsPerRev
        return ((self._rotTarget - self.stepnum)*self.rotDirection)

    def rotateTo(self, target=None,duration=0,direction=0,normalise = True):
        """sets the rotation target and (if direction == 0) calculates if
        it should rotate left or right.
        If target is None moves infinitely in direction 
        (if direction is 0 then hold position with power)."""
        if target == None:
            self._rotTarget = None
            self.rotDirection = direction
        else:
            self._rotTarget = int(target*self.stepsPerRev)
            if normalise:
                rotTargetNorm = self._rotTarget%self.stepsPerRev
                stepsNorm = self.stepnum%self.stepsPerRev
                self._rotTarget = self.stepnum+rotTargetNorm-stepsNorm
                if (self._rotTarget-self.stepnum) > self.stepsPerRev/2:
                    self._rotTarget-=self.stepsPerRev
                elif(self._rotTarget-self.stepnum) < -self.stepsPerRev/2:
                    self._rotTarget+=self.stepsPerRev
            if not self.isAtTarget():
                if (direction == 0):
                    self.rotDirection = 1 if (self._rotTarget > self.stepnum) else -1
                else:
                    self.rotDirection = direction
                self._speedMs = max(int(self.stepDurationMs),duration*1000/self.getStepsToTarget())

    def stop(self):
        self._rotTarget = self.stepnum

    async def _update_async(self):
        while(True):
            await asyncio.sleep_ms(self.stepDurationMs)
            if not self.isAtTarget():
                self._update()
            else:
                self.rotDirection = 0
                self.disablePower()

    def _update(self):
        self.stepnum = (self.stepnum + self.rotDirection)
        self._setPins(stepping[(self.stepnum%8) if not self.inverted else ((8-self.stepnum)%8)])

    def disablePower(self):
        self._setPins([0,0,0,0])

    def getOrientation(self):
        return self.stepnum / self.stepsPerRev

    def getTarget(self):
        return (self._rotTarget / self.stepsPerRev)

    def isAtTarget(self):
        return (self._rotTarget == self.stepnum)