from mechanism import mechanism
import ConfigParserRT
import cortex
import logging
import time

class linear(mechanism):
    """Class representing a general linear mechanism, based on the
    generic mechanism class from mechanism.py.  It adds software motion
    limits and a units/gear ratio property, with corresponding move
    commands enabling moves in terms of physical units rather than motor
    steps."""
    def __init__(self, cortex, icu_num, axis_num, \
                 base_speed, max_speed, acceleration, deceleration, \
                 position, lower_limit, upper_limit, units, name='linear'):
        """
        Initialiser for linear mechanism class.

        Arguments:
            cortex:      String containing the name of the cortex based
                         ICU daisy chain instance that this mechanism is a
                         part of.
            icuNum:      Which ICU in the chain this mechanism is connected
                         to.
            axisNum:     Which axis of the ICU the mechansism is connected
                         to.
            baseSpeed:   Starting speed (units per sec)
            maxSpeed:    Peak speed (units per sec?)
            acceleration:Acceleration (units per sec^2)
            deceleration:Deceleration (units per sec^2)
            position:    Initial position of the mechanism (units)
            lower_limit: Lower limit of allowed movement range (units)
            upper_limit: Upper limit of allowed movement range (units)
            units:       Number of steps corresponding to one unit of movement
            name:        Label used for logging purposes.
        """
        # Store the units
        self.units = float(units)
        # Convert the limits into step based units and store them
        self.lowerLimit = int(float(lower_limit) * self.units)
        self.upperLimit = int(float(upper_limit) * self.units)
        # Convert the remaining parameters into step based units.
        base_speed = int(float(base_speed) * self.units)
        max_speed = int(float(max_speed) * self.units)
        acceleration = int(float(acceleration) * self.units)
        deceleration = int(float(deceleration) * self.units)
        position = int(float(position) * self.units)
        # Call the base class initialiser to deal with the remaining params.
        mechanism.__init__(self, cortex, icu_num, axis_num, \
                           base_speed, max_speed, acceleration, deceleration, \
                           position, name)

    def updateConfig(self, config):
        """
        Should be called by the parent instrument object's destructor
        to enable status information to be saved to file on exit.

        Takes a ConfigParserRT.SafeConfigParser object as argument.
        """
        self.logger.debug("updateConfig() called") 
        if isinstance(config, ConfigParserRT.SafeConfigParser):
            config.set(self.name, 'units', str(self.units))
            config.set(self.name, 'lower_limit', \
                       str(self.lowerLimit / self.units))
            config.set(self.name, 'upper_limit', \
                       str(self.upperLimit / self.units))
            config.set(self.name, 'base_speed', \
                       str(self.baseSpeed / self.units))
            config.set(self.name, 'max_speed', \
                       str(self.maxSpeed / self.units))
            config.set(self.name, 'acceleration', \
                       str(self.acceleration / self.units))
            config.set(self.name, 'deceleration', \
                       str(self.deceleration / self.units))
            config.set(self.name, 'position', \
                       str(self.position / self.units))
        else:
            self.logger.critical("Invalid argument to updateConfig(%s)" \
                                 % repr(config))
            self.logger.critical("Unable to save status data")

    def setupLinear(self):
        """
        Performs an automatic setup procedure.  It is important that the
        limit switches are working correctly otherwise this routine might
        result in damage.
        """
        self.logger.debug('setupLinear() called.')
        # First check that the ICU isn't busy doing something else.
        if self.cortex.isActive():
            self.logger.critical(\
                'Attempt to move mechanism when ICU busy, ignored.')
            self.logger.critical('setupLinear() failed.')
            return

        # Ensure that we're not already against the lower limit.
        lstatus = self.cortex.getLimitStatus(self.icuNum)
        while lstatus == -1:
            mechanism.moveBy(self, +10)
            lstatus = self.cortex.getLimitStatus(self.icuNum)

        # Now find the position of the lower limit switch.
        self.logger.debug('Moving to lower limit.')
        self.cortex.setOutput(self.axisNum, self.icuNum)
        self.cortex.setMotorParams(self.baseSpeed, self.maxSpeed, \
                                   self.acceleration, self.deceleration,
                                   self.icuNum)
        self.cortex.setDatum(self.position)
        self.cortex.moveLimit(-1, self.icuNum)
        while self.cortex.isActive(self.icuNum):
            # Wait until the move is complete.
            time.sleep(1)
        self.position = self.cortex.getPosition(self.icuNum)
        # Quick check we're at the limit we expect to be at...
        lstatus = self.cortex.getLimitStatus(self.icuNum)
        if lstatus == +1:
            self.cortex.setOutput(7, self.icuNum)
            self.logger.critical('Ended up at wrong limit...')
            self.logger.critical('setupLinear() failed.')
            return
        elif lstatus == 0:
            self.cortex.setOutput(7, self.icuNum)
            self.logger.critical('Failed to reach limit switch.')
            self.logger.critical('setupLinear() failed.')
            return
        # Record the position of the lower limit switch.
        lower = self.cortex.getPosition(self.icuNum)
        
        # Now find the position of the upper limit switch.
        self.logger.debug('Moving to upper limit.')
        self.cortex.moveLimit(+1, self.icuNum)
        while self.cortex.isActive(self.icuNum):
            # Wait until the move is complete.
            time.sleep(1)
        self.position = self.cortex.getPosition(self.icuNum)
        # Quick check we're at the limit we expect to be at...
        lstatus = self.cortex.getLimitStatus(self.icuNum)
        if lstatus == -1:
            self.cortex.setOutput(7, self.icuNum)
            self.logger.critical('Ended up at wrong limit...')
            self.logger.critical('setupLinear() failed.')
            return
        elif lstatus == 0:
            self.cortex.setOutput(7, self.icuNum)
            self.logger.critical('Failed to reach limit switch.')
            self.logger.critical('setupLinear() failed.')
            return
        # Record the position of the upper limit switch.
        upper = self.cortex.getPosition(self.icuNum)

        range = upper - lower
        self.position = int(range / 2.0)
        self.upperLimit = int(0.9 * range / 2.0)
        self.lowerLimit = -self.upperLimit

        self.logger.debug('Moving to central position.')
        self.moveTo(0)
            
    def getUnits(self):
        """
        Return the number of steps per unit of movement
        """
        self.logger.debug('getUnits() called')
        return self.units

    def getLimits(self):
        """
        Returns the allowed range of movement of the mechanism as tuple of
        (lowerLimit, upperLimit), in the units specified by setUnits.
        """
        self.logger.debug('getLimits() called')
        return (self.lowerLimit/self.units, self.upperLimit/self.units)

    def getMotorParams(self):
        """
        Returns a tuple containing the four motor motion parameters,
        baseSpeed, maxSpeed, acceleration, deceleration, in terms of
        the units specified with setUnits.
        """
        self.logger.debug('getMotorParams() called')
        return tuple(\
            [item/self.units for item in mechanism.getMotorParams(self)])

    def getPosition(self):
        """
        Returns the current mechanism position, in units defined by
        setUnits().
        """
        self.logger.debug('getPosition() called')
        return mechanism.getPosition(self) / self.units

    def moveBy(self, distance):
        """
        Move the mechanism by a given distance.  Units are defined by
        setUnits()
        """
        self.logger.debug('moveBy(%d) called' % distance)
        # Calculate the new position
        newPosition = self.position / self.units + distance
        # Call moveTo
        self.moveTo(newPosition)

    def moveTo(self, newPosition):
        """
        Move the mechanism to the given position.  Units are defined by
        setUnits().
        """
        self.logger.debug('moveTo(%d) called' % newPosition)
        # Convert the requested position to steps
        newPosition = newPosition * self.units
        # Check software limits
        if newPosition >= self.lowerLimit and newPosition <= self.upperLimit:
            mechanism.moveBy(self, (newPosition - self.position))
        elif newPosition < self.lowerLimit:
            self.logger.critical(\
                'Attempt to move beyond lower limit in moveTo(%d), ignored' \
                % (newPosition / self.units))
        elif newPosition > self.upperLimit:
            self.logger.critical(\
                'Attempt to move beyond upper limit in moveTo(%d), ignored' \
                % (newPosition / self.units))

    def setUnits(self, units):
        """
        Sets the number of steps per unit of movement.
        """
        self.logger.debug('setUnits(%d) called' % units)
        if units > 0:
            self.units = units
        else:
            self.loggger.warning("Negative argument in setUnits(%d), 'fixed'" \
                                 % units)
            self.units = abs(units)

    def setLimits(self, lowerLimit, upperLimit):
        """
        Set the allowed range of movement of the mechanism.  Takes two
        arguments, the lower and upper limits of the range.  Units are
        as defined by setUnits.
        """
        self.logger.debug('setLimits(%d, %d) called' % \
                          (lowerLimit, upperLimit))
        if lowerLimit < upperLimit:
            self.lowerLimit, self.upperLimit = lowerLimit * self.units, \
                                               upperLimit * self.units
        elif upperLimit < lowerLimit:
            self.logger.warning(\
                "Limits wrong way round in setLimits(%d, %d), 'fixed'" \
                % (lowerLimit, upperLimit))
            self.lowerLimit, self.upperLimit = upperLimit * self.units, \
                                               lowerLimit * self.units
        else:
            self.logger.critical(\
                'Lower and upper limits equal in setLimits(%d, %d), ignored' \
                % (lowerLimit, upperLimit))

    def setMotorParams(self, baseSpeed, maxSpeed, acceleration, deceleration):
        """
        Sets the four motor motion parameters, baseSpeed, maxSpeed,
        acceleration and deceleration.   Takes 4 integer arguments.
        Units are as defined by setUnits.
        """
        self.logger.debug('setMotorParams(%i, %i, %i, %i) called' % \
                          (baseSpeed, maxSpeed, acceleration, deceleration))
        mechanism.setMotorParams(self, baseSpeed * self.units, \
                                 maxSpeed * self.units, \
                                 acceleration * self.units, \
                                 deceleration * self.units)

    def setPosition(self, new_position=0):
        """
        Defines the current mechanism position to be equal to the single
        integer argument.  If no argument is given, the mechanism position
        is zeroed.  Units used are as defined in setUnits().
        """
        self.logger.debug('setPosition(%d) called' % new_position)
        mechanism.setPosition(self, new_position * self.units)