#!/usr//bin/env python
# vim: ts=8 sts=4 sw=4 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.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.stopcode = 0
        self.stopCallback = None
        self.moveCallback = None
        self.trace = False

    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
            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'''
        value = self.unitsHome + self.currentSteps / self.stepsPerX
        value = self.getEncoder() - self.absEncHome
        value /= self.cntsPerX
        value += self.unitsHome
        return value

    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
            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
            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
            else:
                self.motorState = "STOPPED"
                self.stopcode = 1
                self.switches = 44 # stopped, both limits clear
            currentPosition = self.getPosition()
            if not self.moveCallback == None:
                self.moveCallback(self)
            # TODO Forward Limit
            # TODO Reverse Limit
            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)))
        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()
        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 == "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 doCommandST(self, arg):
        if self.motorState in ["MOVING"]:
            self.motorState = "STOPPED"
            self.stopcode = 4
            self.switches = 44
            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.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