######################  axis.rb -- brent@mbari.org  ######################
# $Source: /home/cvs/ESP/gen2/software/esp/lib/axis.rb,v $
#    Copyright (C) 2003 MBARI
#    MBARI Proprietary Information. All rights reserved.
# $Id: axis.rb,v 1.35 2005/06/24 18:26:35 brent Exp $
#
#     Rotary Shaft control
#
#  Generic help for control axes
#
############################################################################

require 'delay'    #for duration parsing
require 'axismap'  #generic position map with aliasing

class Axis < AxisKernel 

  class Error < AxisKernel::Error; end
    
  def initialize(axisName, dwarf, channelNumber, cfg, maxDuration, slop)
  #initialize a new Axis object
  #dwarf is the network target that receives replies and sources requests
    coreInit axisName
    @maxDuration = maxDuration  #max seconds to wait for rotation request
    @channel=channelNumber
    @slop=slop
    @dwarf=dwarf
    @config=cfg
  end
  
  def coreInit(axisName)
  #initialize the core of a new Axis object
    @name=axisName
    @map=AxisMap.new Hash.new  #seed object with an empty map
    @sortedMap=[]
    type.define self
  end
  protected :coreInit
  
  attr_accessor :name, :slop, :config
  attr_reader :map, :dwarf, :channel, :maxDuration

  def maxDuration= time
    @maxDuration=Delay.new time
  end
  
  def to_s
    position.to_s
  end
  def asIRBtext
    position.asIRBtext
  end
  
  def positions
    @sortedMap.size
  end
  
  def legend
    @map.legend
  end

  def labels
    @map.labels
  end

  def aliases
    @map.aliases
  end
  
  def with axisMap
  #designate axis positions (as an AxisMap)
    begin
      @sortedMap = (@map=axisMap).labels.invert.sort
    rescue AxisMap::Error=>err
      axisErr err
    end
    self
  end
  alias_method :detents, :with

  def alias hash={}
  #add or change aliases for existing positions
    begin
      @map.alias hash
    rescue AxisMap::Error=>err
      axisErr err
    end    
  end
  
  def unalias hash={}
  #remove an alias
    begin
      @map.unalias hash
    rescue AxisMap::Error=>err
      axisErr err
    end    
  end
  
  def relabel newLabel, oldId
  #replace label for position oldId with newLabel
    begin
      rawPos = @map.relabel newLabel, oldId
    rescue AxisMap::Error=>err
      axisErr err
    end    
    @sortedMap[rawIndex(rawPos)][1] = newLabel
    rawPos
  end
  
  def fetch(positionId, &punt)
    begin
      @map.fetch (positionId, &punt)
    rescue AxisMap::Error=>err
      axisErr err
    end    
  end

  def raw goal
    fetch(goal) do |g|
      if g.respond_to? :raw
        compatible? goal.axis
        g.raw
      else
        baseRaw g
      end
    end
  end

  def asPosition goal
  #return goal as a position object on a compatible axis
    if goal.respond_to? :raw
      verifyCompatibility goal.axis
      goal
    else
      at goal
    end
  end
    
  def compatible? axis
    @map==axis.map
  end
  
  def verifyCompatibility axis
    unless compatible? axis
      raise Error.new (
        "#{@name} and #{axis.name} are different axes!", self)
    end
  end

  def rawIndex counts
  #given raw position in counts, return index into sortedMap of names
  #return a negative index if not found.  In this case
  #  (-result-1)  is the index of the nearest value > counts
  #  (-result-2)  is the index of the nearest value < counts
    lowBound = 0; highBound = @sortedMap.size-1
    while lowBound <= highBound
      index = (lowBound+highBound)/2      
      return index if counts == (probe=@sortedMap[index].first)
      if probe > counts
        highBound = index-1
      else
        lowBound = index+1
      end
    end
    -lowBound-1  #return -index of nearest value > counts
  end
  
  def configure
    begin
      @dwarf.configure @channel, @config
    rescue =>err
      err.message[0,0]="Cannot configure #{@name}:  "
      raise
    end
  end

  def getConfig
  #get configuration from the Servo
    begin
      @dwarf.getConfig @channel
    rescue =>err
      err.message[0,0]="Failed reading #{@name}'s configuration:  "
      raise
    end
  end
  
  def status
  #return the current raw status of the shaft
    begin
      @dwarf.status @channel
    rescue =>err
      err.message[0,0]="Failed reading #{@name}'s status:  "
      raise
    end
  end
    

  module Utils  #common utilities for positions on all axes
    attr_reader :axis
    def compatible? other
      @axis.compatible? other.axis
    end
    def == other
      compatible?(other) and raw==other.raw
    end
    
    def advance detents=1, slop=@axis.slop
      index = @axis.rawIndex(@axis.raw(self), slop) + detents
      @axis.rawId (@axis.indexRaw(index))
    end
    alias_method :forward, :advance
    
    def retard detents=1, slop=@axis.slop
      advance(-detents, slop)
    end
    alias_method :backward, :retard
  end

  class Between  #position is between two named positions
    include Utils
    def initialize axis, start, terminus
      @axis = axis; @begin = start; @end = terminus
    end
    def to_s
      "between #{@begin} and #{@end}"
    end
    def asIRBtext
      "#{@axis.name} #{self}"
    end
    attr_reader :begin, :end
    def offset
      nil
    end
  end

  class Position  #position near a named position
    include Utils
    def initialize axis, base, offset=nil
      @axis = axis; @base = base; @offset = offset
    end
    @@suffix = {nil=>'', 1=>' + 1 count', -1=>' - 1 count'}
    def to_s
      "#{@base}"+@@suffix.fetch(@offset) {|off|
        " #{off<0 ? '-' : '+'} #{off.abs} counts"
      }
    end
    def asIRBtext
      "#{@axis.name} at #{self}"
    end
    attr_reader :base, :offset
    def + bias
      off = 0 unless off = @offset
      type.new (@axis, @base, off+bias)
    end
    def - bias
      if bias.kind_of? Numeric
        self + -bias
      else
        @axis.verifyCompatibility bias.axis
        raw - bias.raw
      end
    end
  end  #Axis::Position class

  def at base, offset=nil
    (type.const_get :Position).new self, base, offset
  end
  alias_method :[],:at
  
  def between start, terminus
    (type.const_get :Between).new self, start, terminus
  end
  
  def advance detents=1, slop=@slop
  #return next named position (circularly)
  #detents may be negative or even a Float
    seek (position.advance detents, slop)
  end
  alias_method :forward, :advance

  def retard detents=1, slop=@slop
    advance(-detents, slop)
  end
  alias_method :backward, :retard
  
  protected
  def baseRaw base
  #superclasses may redefine this to compute a mapping of base positions
  #onto raw counts.  But, we punt by default...
    raise Error.new "Unknown #{@name} position: #{base}", self
  end
  alias_method :posErr, :baseRaw

  def axisErr err
  #(re)raise specified err prefixed by the axis name
    raise err.exception(name.to_s+': '+err.message)
  end

end  #Axis class


class LinearAxis < Axis

  class Error < Axis::Error; end

  module Utils
    include Axis::Utils
    def seek maxDuration=@axis.maxDuration
      @axis.seek self, maxDuration
    end    
    alias_method :moveTo, :seek
  end

  class Between < Axis::Between
    include Utils
    def raw
      b = @axis.raw self.begin
      e = @axis.raw self.end
      (b + e) / 2
    end
  end
  
  class Position < Axis::Position
    include Utils
    def raw
      @axis.raw(base) + (@offset ? @offset : 0)
    end
  end

  def with axisMap
  #linear axes must have at least two positions to serve as limits
    super
    if positions < 2
      raise Error.new (
        "#{name} requires at least two positions to define its limits", self)
    end
    self
  end

  def maxRaw
    @sortedMap[-1].first
  end
  def maxPosition
    @sortedMap[-1].last
  end
  
  def minRaw
    @sortedMap[0].first
  end
  def minPosition
    @sortedMap[0].last
  end
  
  def rawIndex counts, slop=@slop
  #given raw position in counts, return index into sortedMap of names
  #if raw position within slop of any position, return an Integer, otherwise
  #return a Float whose Integer part selects the first detent > counts
    i=super counts
    return i if i >= 0  #exact match!
    lesser = (greater = -i-1) - 1
    if lesser < 0 #less than 1st position -- may be near to it
      return 0 if minRaw - counts <= slop
      lesser = 0
      extent = maxRaw - (base=minRaw)
    else      
      if greater >= (sz=@sortedMap.size) #may be near last position
        return sz-1 if counts - maxRaw <= slop
        lesser = sz-1
        extent = (base=maxRaw) - minRaw
      else
        base=@sortedMap[lesser].first
        limit=@sortedMap[greater].first
        if (boff=counts-base) < (loff=limit-counts) #nearer to base than limit
          return lesser if boff <= slop
        else
          return greater if loff <= slop
        end
        extent = limit - base
      end
    end
    Float(counts - base) / extent + lesser
  end
  

  def indexRaw index
  #inverse of rawIndex
  #return raw counts given index into position sortedMap   
    if index < 0 or index > (limit = @sortedMap.size-1)
      raise Error.new ("#{name} [#{index}] out of bounds", self)
    end
    return @sortedMap[limit] if index == limit
    fraction = index - (i = Integer index)
    extent = @sortedMap[i.succ].first - (base = @sortedMap[i].first)
    Integer((base + fraction*extent).round)
  end

  
  def rawId counts, slop=@slop
  #given raw position in counts, return:
  # corresponding position name if within slop counts of any name
  # Between if within slop counts of the average of two named positions
  # otherwise a Position   
    i = Integer (index = rawIndex(counts, slop))
    if i<0
      i=0
    else
      i=@sortedMap.size-1 if i>=@sortedMap.size
    end
    if index == i
      at (@sortedMap[i].last)
    else
      posBefore = nearest = @sortedMap[i]
      if posAfter = @sortedMap[i.succ]
        center = ((b = posBefore[0])+(a = posAfter[0])) / 2
        return between (posBefore[1], posAfter[1]) if 
          (counts - center).abs <= slop
        nearest = posAfter if (a - counts).abs < (b - counts).abs 
      end
      at (nearest[1], counts - nearest[0])
    end
  end
  alias_method :baseRawId, :rawId  #baseRawId should not be overriden

  def moveBetween start, terminus, maxDuration=@maxDuration
    seek(between(start,terminus), maxDuration)
  end
  
end


class RotaryAxis < Axis  #revolving axis

  class Error < Axis::Error; end
  
  alias_method :angle, :raw
    
  module Utils
    include Axis::Utils
    def reduce
      #reduce angle to connonical form removing aliasing
      @axis.angleId angle
    end
    def dial direction=nil, maxDuration=@axis.maxDuration
      @axis.dial self, direction, maxDuration
    end    
    alias_method :rotateTo, :dial
    alias_method :select, :dial
  end

  class Between < Axis::Between
    include Utils
    def Between.twoRawAngles b, e, revCounts
    #return the raw angle "between" two given raw angles
      halfRev = revCounts / 2
      c = (b + e) / 2
      c += halfRev if (b - e).abs > halfRev #always go the short way round
      c -= revCounts if c >= revCounts
      c
    end     
    def angle
      type.twoRawAngles @axis.raw(self.begin), 
                        @axis.raw(self.end), @axis.config.revCounts
    end
    alias_method :raw, :angle
  end
  
  class Position < Axis::Position
    include Utils
    def angle
      b = @axis.raw base
      (@offset and @offset != 0) ? (b+@offset) % @axis.config.revCounts : b
    end
    alias_method :raw, :angle
  end

  def with axisMap
  #designate rotary positions (as an AxisMap)
    super
    lastAngle = @sortedMap[-1].first
    revCounts = @config.revCounts
    raise Error.new(
     "#{name} Angle(#{lastAngle}) >= Counts/Revolution(#{revCounts})", self) if
        lastAngle >= revCounts
    firstAngle = @sortedMap[0].first
    raise Error.new("#{name} Angle(#{firstAngle}) < 0", self) if firstAngle < 0
    self
  end
  alias_method :detents, :with


  def shortest angle1, angle2
  #shortest (rotational) distance between angle1 and angle2
    off = (angle1 - angle2).abs
    wrap = @config.revCounts - off
    off < wrap ? off : wrap
  end
  private :shortest
  
  def rawIndex rawAngle, slop=@slop
  #given raw angle in counts, return index into sortedMap of corresponding names
  #if angle within slop of any detent, return an Integer, otherwise
  #return a Float whose Integer part selects the first detent > angle
    revCounts = @config.revCounts
    i=super (rawAngle %= revCounts)
    return i if i >= 0  #exact match!
    lesser = (greater = -i-1) - 1
    sz=@sortedMap.size
    if lesser < 0 or greater >= sz  #near 0
      base=@sortedMap[0].first
      limit=@sortedMap[last = sz-1].first
   
      boff = shortest rawAngle, base      
      loff = shortest rawAngle, limit
      if boff < loff   #nearer to base than limit
        return 0 if boff <= slop
      else
        return last if loff <= slop
      end
      extent = @config.revCounts + base - limit
       # index returned between -1 and 0 if angle less than all positions
      lesser = lesser < 0 ? 0 : (base=limit; last)
      
    else
    
      base=@sortedMap[lesser].first
      limit=@sortedMap[greater].first
      if (boff=rawAngle-base) < (loff=limit-rawAngle) #nearer to base than limit
        return lesser if boff <= slop
      else
        return greater if loff <= slop
      end
      extent = limit - base
      
    end
    Float(rawAngle - base) / extent + lesser
  end
  alias_method :angleIndex, :rawIndex
  

  def indexAngle index
  #inverse of angleIndex
  #return angle in counts given index into position sortedMap
    index %= (sz=@sortedMap.size)
    fraction = index - (i = Integer index)
    revCounts = @config.revCounts
    r = 0
    (i = -1; r = revCounts) if i >= sz-1
    extent = @sortedMap[i.succ].first - (base = @sortedMap[i].first) + r
    Integer((base + fraction*extent).round) % revCounts
  end
  alias_method :indexRaw, :indexAngle

  
  def angleId angle, slop=@slop
  #given raw angle in counts, return:
  # corresponding position name if within slop counts of any name
  # Between if within slop counts of the average of two named positions
  # otherwise the nearest Position +/- offset
    index = angleIndex angle, slop
    if index == (i = Integer index)
      at (@sortedMap[i].last)
    else
      revCounts = @config.revCounts
      if index < 0 or (j=i.succ) >= @sortedMap.size
        posBefore = @sortedMap[-1]; posAfter = @sortedMap[0]
        r = revCounts
      else
        posBefore = @sortedMap[i]; posAfter = @sortedMap[j]
        r = 0
      end
      rawBetween = 
        Between.twoRawAngles (b=posBefore.first, a=posAfter.first, revCounts)
      if shortest (angle%revCounts, rawBetween) <= slop
        between posBefore.last, posAfter.last
      else
        nearest = (b - angle).abs < (a+r - angle).abs ? posBefore : posAfter
        at (nearest.last, angle - nearest.first)
      end
    end
  end
  alias_method :rawId, :angleId

end