# FilterWheel
from EZStepper import CommandResponse, MotorMoveRel, MotorReadPosition, MotorWait, MotorSetOrigin
class FilterWheel:
    def __init__(self, SerialPort, TimeOut=60, ControllerName=1, FilterPositions=22, MotorSteps=200, HoldCurrent=10, RunCurrent=95, \
                 StepDivide=16, SlewVelocity=5000, InitVelocity=100, Ramp=2, OffsetFromHome=-2):
        self.SerialPort = SerialPort # serial port object; must be open
        self.TimeOut = TimeOut # duration of longest move
        self.ControllerName = ControllerName # name (number) of controller on this port
        self.FilterPositions = FilterPositions # number of filter positions in the wheel
        self.MotorSteps = MotorSteps # physical steps in the motor; typically 200
        self.HoldCurrent = HoldCurrent # current when not moving; in % of half of full current (2A for EZHR17EN); typically 5-10%
        self.RunCurrent = RunCurrent # current when moving; in % of full current (2A for EZHR17EN); typically 50-95%
        self.StepDivide = StepDivide # number of microsteps per step; typically 16 for filter wheels
        self.SlewVelocity = SlewVelocity # slew (max) velocity in microsteps/sec 
        self.InitVelocity = InitVelocity # initial velocity in microsteps/sec 
        self.Ramp = Ramp # acceleration (deceleration) in microsteps/sec^2 
        self.OffsetFromHome = OffsetFromHome # offset from home to the position of the first filter in microsteps
        self.mspf = 0 # microsteps per filter; calculated during initialization (homing)
        self.f0pos = 0 # position of the first filter (fnum=1) in microsteps; calculated during initialization (homing)
        self.fnum = 0 # current filter number; set by homing and by moves; 1-based
        self.msteppos = 0 # current position in microsteps (mirrors position counter in controller)

    def WheelHomeOwn2(self):
        serport = self.SerialPort
        conname = self.ControllerName
        timeout = self.TimeOut
        cmnd = 'gD10S13G0D1gD1S03G0D1R' # custom homing sequence
        status, response = CommandResponse(serport, conname, cmnd)
        errh = MotorWait(serport, conname, timeout)
        return errh

    def WheelSetParameters(self):
        serport = self.SerialPort
        conname = self.ControllerName
        holdcurrent = self.HoldCurrent
        runcurrent = self.RunCurrent
        stepdivide = self.StepDivide
        slewvelocity = self.SlewVelocity
        initvelocity = self.InitVelocity
        ramp = self.Ramp
        cmnd = 'h' + str(holdcurrent) + 'm' + str(runcurrent) + 'j' + str(stepdivide) + 'V' + str(slewvelocity) + 'v' + str(initvelocity) + 'L' + str(ramp) + 'R'
        status, response = CommandResponse(serport, conname, cmnd)
        return status

    def WheelHome(self):
        serport = self.SerialPort
        conname = self.ControllerName
        stepdivide = self.StepDivide 
        timeout = self.TimeOut
        offsetfromhome = self.OffsetFromHome
        motorsteps = self.MotorSteps
        filterpositions = self.FilterPositions
        status1 = FilterWheel.WheelSetParameters(self)
        status2 = MotorSetOrigin(serport, conname, stepdivide, timeout)
        errh = FilterWheel.WheelHomeOwn2(self)
        status3, response = MotorMoveRel(serport, conname, offsetfromhome)
        errw = MotorWait(serport, conname, timeout)
        errp, f0pos = MotorReadPosition(serport, conname)
        mspf = motorsteps * stepdivide / filterpositions
        fnum = 1
        err = errh + errw*10 + errp*100
        self.mspf = mspf
        self.f0pos = f0pos
        self.fnum = fnum
        self.msteppos = f0pos
        return err
  
    def WheelMove(self, newfnum):
        # move wheel to filter position newfnum; return error code
        # filter numbering is 1-based
        # it is assumed that wheel is not moving when this function is called; use WheelWait() before it
        serport = self.SerialPort
        conname = self.ControllerName
        fnum = self.fnum
        filterpositions = self.FilterPositions
        motorsteps = self.MotorSteps
        stepdivide = self.StepDivide
        mspf = self.mspf
        f0pos = self.f0pos
        if(fnum == newfnum): return 0
        if(newfnum < 1): return -1
        if(newfnum > filterpositions): return -2
        if(newfnum != round(newfnum)): return -3
        turnsteps = motorsteps*stepdivide
        # errp, pos = MotorReadPosition(serport, conname)
        pos = self.msteppos # this is faster than reading from controller in the commented line above
        apos = pos - f0pos
        cpos = apos%turnsteps # current position normalized
        tpos = (newfnum-1)*mspf # target position
        if(tpos < cpos): 
            tpos = tpos+turnsteps
        delta1 = tpos-cpos
        delta2 = delta1-turnsteps
        if(abs(delta1) < abs(delta2)):
            delta = delta1
        else: 
            delta = delta2
        status, response = MotorMoveRel(serport, conname, delta)
        self.fnum = newfnum # setting the filter number towards which the wheel is moving
        self.msteppos += sign(delta)*round(abs(delta)) # setting the microstep position towards which the wheel is moving
        return status

    def WheelWait(self):
        serport = self.SerialPort
        conname = self.ControllerName
        timeout = self.TimeOut
        return MotorWait(serport, conname, timeout)

    def WheelPos(self):
        # return current filter position, or error (-1) if wheel is not at a correct position
        serport = self.SerialPort
        conname = self.ControllerName
        filterpositions = self.FilterPositions
        mspf = self.mspf
        f0pos = self.f0pos
        eps = 0.02 # allowed error due to possible non-integer number of microsteps between filter positions
        errp, pos = MotorReadPosition(serport, conname)
        fp = (pos - f0pos) / mspf
        rfp = round(fp)
        if(abs(fp-rfp) > eps): return -1
        if(rfp >= 0):
            fnum = rfp % filterpositions + 1
        else:
            fnum = filterpositions - (-rfp % filterpositions) + 1
        if(fnum > filterpositions): fnum = fnum % filterpositions
        return fnum

    def WheelNext(self):
        newfnum = self.fnum + 1
        if(newfnum > self.FilterPositions):
            newfnum = 1
        return FilterWheel.WheelMove(self, newfnum)
    
    def WheelPrev(self):
        newfnum = self.fnum - 1
        if(newfnum < 1):
            newfnum = self.FilterPositions
        return FilterWheel.WheelMove(self, newfnum)

def sign(a):
    if(a>=0): 
        return 1
    else: 
        return -1