#!/usr//bin/env python
# vim: ts=8 sts=4 sw=4 ft=python expandtab
# Author: Douglas Clowes (dcl@ansto.gov.au) 2012-06-29
import inspect
import time
import math

class GalilMotor(object):
    """Galil Motor Object"""

    def __init__(self, theAxis):
        print "GalilMotor ctor: %s" % theAxis
        self.name = None
        self.axis = theAxis
        self.motorState = "OFF"
        self.stepsPerX = 25000.0
        self.cntsPerX = 8192.0
        self.speed = 25000
        self.accel = 2500
        self.decel = 2500
        self.lowerLim = -20.0
        self.upperLim = 20.0
        self.hasAbsEnc = True
        self.unitsHome = 0.0
        self.absEncHome = 100 * 1000 * 1000
        self.motorHome = 0.0
        self.currentSteps = 0
        self.targetSteps = self.currentSteps
        self.configDone = False
        self.switches = 44
        self.fwdlim = 1
        self.revlim = 1
        self.stopcode = 0
        self.stopCallback = None
        self.moveCallback = None
        self.trace = False
        self.rotary_bits = 0

    def doTrace(self, arg):
        print "Trace %s: %s" % (self.axis, arg),
        if self.trace:
            print "(ON =>",
        else:
            print "(OFF =>",
        if arg == "ON":
            self.trace = True
        else:
            self.trace = False
        if self.trace:
            print "ON)"
        else:
            print "OFF)"

    def doConfigure(self, arg):
        print "Configure %s: %s" % (self.axis, arg)
        the_parts = arg.split(",")
        configDoneAlready = self.configDone
        for part in the_parts:
            (lh, rh) = part.split("=")
            if lh == "SPX":
                self.stepsPerX = float(rh)
                self.configDone = True
            elif lh == "CPX":
                self.cntsPerX = float(rh)
                self.configDone = True
            elif lh == "RL":
                self.lowerLim = float(rh)
                self.configDone = True
            elif lh == "FL":
                self.upperLim = float(rh)
                self.configDone = True
            elif lh == "UH":
                self.unitsHome = float(rh)
                self.configDone = True
            elif lh == "EH":
                self.hasAbsEnc = True
                self.absEncHome = float(rh)
                self.configDone = True
            elif lh == "MH":
                self.hasAbsEnc = False
                self.motorHome = float(rh)
                self.configDone = True
            elif lh == "NAM":
                self.name = rh[1:-1]
                self.configDone = True
            elif lh == "ROT":
                self.rotary_bits = int(rh)
                self.configDone = True
            else:
                print "Unrecognized config item \"%s\" in \"%s\"" % (lh, arg)
        if self.configDone and not configDoneAlready:
            self.currentSteps = 0
            self.targetSteps = 0

    def getEncoder(self):
        '''Calculate the encoder from the currentSteps'''
        value = self.absEncHome + self.currentSteps * (self.cntsPerX / self.stepsPerX)
        return int(value)

    def getPosition(self):
        '''Calculate the unit position'''
        if self.hasAbsEnc and self.cntsPerX != 0:
            # absolute encoder case
            value = self.getEncoder() - self.absEncHome
            value /= self.cntsPerX
            value += self.unitsHome
            return value
        elif self.stepsPerX != 0:
            # handle non-AbsEncoder case
            value =  self.currentSteps - self.motorHome
            value /= self.stepsPerX
            return value
        else:
            # not-configured case
            return -123.456

    def doIteration(self, now):
        '''If moving then update speed and position using
            v = u + at
            s = ut + 1/2 at**2 = (u + v)t / 2
            v**2 = u**2 + 2as
        '''
        if self.motorState in ["MOVING"]:
            t = now - self.motorStartTime
            distance = abs(self.targetSteps - self.motorStartSteps)
            topSpeed = math.sqrt(2.0 * distance / ( 1.0 / self.accel + 1.0 / self.decel))
            if topSpeed < 1e-6:
                topSpeed = 0
            if topSpeed > self.speed:
                topSpeed = self.speed
            t1 = topSpeed / self.accel
            s1 = 0.5 * self.accel * t1**2
            t3 = topSpeed / self.decel
            s3 = 0.5 * self.decel * t3**2
            s2 = distance - s1 - s3
            if s2 < 1e-6:
                s2 = 0
            if s2 == 0 or topSpeed == 0:
                t2 = 0
            else:
                t2 = s2 / topSpeed
            if t >= (t1 + t2 + t3):
                # Finished
                s = distance
                v = 0
                self.motorState = "STOPPED"
                self.stopcode = 1
                self.switches = 44 # stopped, both limits clear
                self.fwdlim   = 1
                self.revlim   = 1
            elif t >= (t1 + t2):
                # Ramp Down
                dt = t - (t1 + t2)
                # v = u + at
                v = topSpeed - self.decel * dt
                # s = (u + v)t / 2
                s = s1 + s2 + 0.5 * (topSpeed + v) *  dt
            elif t >= t1:
                # Run Flat
                v = topSpeed
                s = s1 + topSpeed * (t - t1)
            else:
                # Ramp Up
                v = 0.0 + self.accel * t
                s = 0.5 * self.accel * t**2
            if self.speed < 1e-6:
                self.percentVelocity = 100.0
            else:
                self.percentVelocity = 100.0 * v / self.speed
            if distance < 1e-6:
                self.percentDistance = 100.0
            else:
                self.percentDistance = 100.0 * s / distance
            if self.targetSteps > self.motorStartSteps:
                self.currentSteps = self.motorStartSteps + int(s)
                if self.currentSteps >= self.targetSteps:
                    self.motorState = "STOPPED"
                    self.stopcode = 1
                    self.switches = 44 # stopped, both limits clear
                    self.fwdlim   = 1
                    self.revlim   = 1
            elif self.targetSteps < self.motorStartSteps:
                self.currentSteps = self.motorStartSteps - int(s)
                if self.currentSteps <= self.targetSteps:
                    self.motorState = "STOPPED"
                    self.stopcode = 1
                    self.switches = 44 # stopped, both limits clear
                    self.fwdlim   = 1
                    self.revlim   = 1
            else:
                self.motorState = "STOPPED"
                self.stopcode = 1
                self.switches = 44 # stopped, both limits clear
                self.fwdlim   = 1
                self.revlim   = 1
            currentPosition = self.getPosition()
            if not self.moveCallback == None:
                self.moveCallback(self)
            if self.targetSteps > self.motorStartSteps and currentPosition >= self.upperLim:
                # Forward Limit
                self.motorState = "STOPPED"
                self.stopcode = 2 # stopped by forward limit
                self.switches = 44 - 8 # stopped, reverse limit clear
                self.fwdlim   = 0
                self.revlim   = 1
                pass
            if self.targetSteps < self.motorStartSteps and currentPosition <= self.lowerLim:
                # Reverse Limit
                self.motorState = "STOPPED"
                self.stopcode = 3 # stopped by reverse limit
                self.switches = 44 - 4 # stopped, forward limit clear
                self.fwdlim   = 1
                self.revlim   = 0
                pass
            if self.motorState == "STOPPED":
                print "Axis %s stopped at:" % self.axis, currentPosition
                self.currentSteps = self.targetSteps
                if not self.stopCallback == None:
                    self.stopCallback(self)
        else:
            pass

    def doAssignment(self, cmd, arg):
        '''This is for commands like "PAA=1234567" which are for the motor.
           The axis and the assignment operator have already been removed.
           '''
        if self.trace:
            print "Trace ASG %s: %s" % (self.axis, str(cmd) + " " + str(arg))
        if cmd == "PA":
            self.targetSteps = int(arg)
        elif cmd == "PR":
            self.targetSteps = self.currentSteps + int(arg)
        elif cmd == "SP":
            self.speed = max(1, abs(int(arg)))
        elif cmd == "AC":
            self.accel = max(1, abs(int(arg)))
        elif cmd == "DC":
            self.decel = max(1, abs(int(arg)))
        elif cmd == "DP":
            self.currentSteps = int(arg)
        else:
            print "Unknown assignment", cmd, arg

    def doQuery(self, arg):
        if arg == "TD":
            response = self.currentSteps
        elif arg == "ZZ":
            response = self.getPosition()
        elif arg == "TP":
            response = self.getEncoder()
            if self.rotary_bits > 0:
                response = response & ((1 << self.rotary_bits) - 1)
        elif arg == "SP":
            response = self.speed
        elif arg == "AC":
            response = self.accel
        elif arg == "DC":
            response = self.decel
        elif arg == "TS":
            response = self.switches
        elif arg == "LF":
            response = self.fwdlim
        elif arg == "LR":
            response = self.revlim
        elif arg == "SC":
            response = self.stopcode
        elif arg == "MO":
            if self.motorState in ["OFF"]:
                response = 1
            else:
                response = 0
        elif arg == "BG":
            if self.motorState in ["MOVING"]:
                response = 1
            else:
                response = 0
        else:
            print "Unknown Query:", arg
            response = None
        if self.trace:
            print "Trace QRY %s: %s" % (self.axis, str(arg) + " = " + str(response))
        return response

    def doCommandTD(self, arg):
        return self.doQuery("TD")

    def doCommandZZ(self, arg):
        result = self.doQuery("ZZ")
        print "Position:", result
        return result

    def doCommandTP(self, arg):
        return self.doQuery("TP")

    def doCommandSC(self, arg):
        return self.doQuery("SC")

    def doCommandTS(self, arg):
        return self.doQuery("TS")

    def doCommandST(self, arg):
        if self.motorState in ["MOVING"]:
            self.motorState = "STOPPED"
            self.stopcode = 4
            self.switches = 44
            self.fwdlim   = 1
            self.revlim   = 1
            return True

    def doCommandBG(self, arg):
        if self.motorState in ["ON", "STOPPED"]:
            self.motorState = "MOVING"
            self.stopcode = 0
            self.switches = 140 # moving, both limits clear
            self.fwdlim   = 1
            self.revlim   = 1
            self.motorStartTime = time.time()
            self.motorStartSteps = self.currentSteps
            return True
        return False

    def doCommandSH(self, arg):
        if self.motorState in ["ON", "OFF", "STOPPED"]:
            self.motorState = "ON"
            return True
        return False

    def doCommandMO(self, arg):
        if self.motorState in ["ON", "OFF", "STOPPED"]:
            self.motorState = "OFF"
            return True
        return False

    def doCommandPA(self, arg):
        self.targetSteps = int(arg)
        return True

    def doCommandPR(self, arg):
        self.targetSteps = self.currentSteps + int(arg)
        return True

    def doCommand(self, cmd, arg):
        '''This is for commands like "BGA" which are for the motor.
           The axis and other cruft have already been removed.
           '''
        action = "inspect.ismethod(GalilMotor.doCommand%s)" % cmd
        if eval(action):
            if self.trace:
                data = cmd
                if arg:
                    data = data + " " + arg
                print "Trace CMD: \"" + data + "\""
            action = "response = self.doCommand%s(%s)" % (cmd, arg)
            exec action
            return response
        else:
            print "Unknown Action:", eval(action), "for cmd:", action

if __name__ == '__main__':
    motor = GalilMotor("A")
    motor.doConfigure("SPX=25000.0,CPX=8192.0,RL=-10.0,FL=10.0,UH=0.0,EH=100000000")
    print "SP", motor.doAssignment("SP", "25000")
    print "AC", motor.doAssignment("AC", "25000")
    print "DC", motor.doAssignment("DC", "25000")
    print "PA", motor.doAssignment("PA", "100000")
    print "SH", motor.doCommand("SH", None)
    print "BG", motor.doCommand("BG", None)
    print "MO", motor.doQuery("MO")
    print "BG", motor.doQuery("BG")
    count = 0
    while motor.doQuery("BG") and count <= 100:
        t = 0.1 * count
        motor.doIteration(motor.motorStartTime + t)
        rnge = 70
        vspot = int(0.01 * rnge * motor.percentVelocity)
        sspot = int(0.01 * rnge * motor.percentDistance)
        bth = "b"
        if sspot >= 0:
            ess = "s"
        else:
            ess = "*"
            bth = "B"
            sspot = 0
        if vspot >= 0:
            vee = "v"
        else:
            vee = "^"
            bth = "B"
            vspot = 0
        if vspot < sspot:
            line = " "*(vspot) + vee + " "*(sspot - vspot - 1) + ess + " "*(rnge - sspot) + "|"
            line += " %2d %2d" % (vspot, sspot)
        elif vspot > sspot:
            line = " "*(sspot) + ess + " "*(vspot - sspot - 1) + vee + " "*(rnge - vspot) + "|"
            line += " %2d %2d" % (sspot, vspot)
        else:
            line = " "*(sspot) + bth + " "*(rnge - sspot) + "|"
            line += " %2d" % (sspot)
        print "%3.2f: %s" % (t, line)
        count += 1
    print "_MO", motor.doQuery("MO")
    print "_BG", motor.doQuery("BG")
    print "TP", motor.doCommand("TP", None)
    print "TD", motor.doCommand("TD", None)
    print "ZZ", motor.doCommand("ZZ", None)
    print "_ZZ", motor.doQuery("ZZ")
    print "MO", motor.doCommand("MO", None)
    print "_MO", motor.doQuery("MO")
    print "Iterations:", count