# miscelleneous utilities

module Arm
#complete manipulator arm assembly (Hand, Forearm, and Elbow)

  def Arm.to destination, forearmOffset=ForearmPickOffset
    unless Elbow.position == Elbow.at(destination)
      retractForearm
      Elbow.to destination, TightElbow
    end
    extend forearmOffset
    self
  end
  
  def Arm.grab start=nil, destination=nil
    to start if start
    unless Hand.closed?
      Forearm.reconfigure LooseForearm
      Hand.close
    end
    to destination if destination
    self
  end
  
  def Arm.retractForearm
    Forearm.to :retract, QuickForearm unless 
      Forearm.position == Forearm.at(:retract)
  end
  
  def Arm.retract
    Hand.open unless Hand.opened?
    retractForearm
    self
  end

  def Arm.releaseAt destination
    to destination, nil
    retract
  end
  
  def Arm.extend forearmOffset=ForearmPickOffset
    elbowPos = Elbow.position
    raise "Elbow is not aligned for Arm.extend!" if elbowPos.offset
    Elbow.reconfigure LooseElbow    #let the arm conform to the mechanism
    unless elbowPos.base.kind_of? Integer  #not pointed to elevator
      Forearm.to Elbow.position.base, 
        Hand.opened? ? LooseForearm : ClampForearm
    else  #compensate forearm position for the selected carousel tube
      tubePos = SC.position
      raise "Carousel is not aligned to a tube!" if tubePos.offset
      Forearm.to Forearm[tubePos.base, forearmOffset], 
           (Hand.opened? or forearmOffset) ? TightForearm : LooseForearm
    end
    self
  end
  
  def Arm.place forearmOffset=nil
  #extend arm to guide pucks into the currently selected elevator tube
    to SC.position.base, forearmOffset
  end
  
  def Arm.pick
    place ForearmPickOffset  #back off from "place" extension
  end
  
  def Arm.stackPuck forearmOffset=ForearmPickOffset
    place
    Hand.open unless Hand.opened?
    extend forearmOffset if forearmOffset  #to let puck stack slide by palm
  end
  
  def Arm.ready!
    Forearm.home.to :retract, QuickForearm
    closing = Thread (:CloseHand) {Hand.close}
    begin  #in case elbow is on the "wrong" side of its home flag
      Elbow.cancelHome.jog ElbowSafetyBump
    rescue Slide::Error  #ignore any crash into an elbow limit
      Elbow.hold
    end
    Elbow.home.to :home  #now, it's safe to home the elbow normally
    closing.finish
    self
  end
  
  def Arm.extended? elbowPosition=Elbow.position, forearmPosition=Forearm.position
  #return true if Arm is extended properly for current Elbow position
    elbowBase = elbowPosition.base
    return false if Elbow[elbowBase] != elbowPosition  #Elbow out of position
    (Forearm[elbowBase].raw - forearmPosition.raw).abs <= Forearm.slop
  end
  
  def Arm.retracted? forearmPosition=Forearm.position
    forearmPosition == Forearm[:retracted]
  end
  
  def Arm.at? station
  #return true iff Arm is extended at the specified processing station
    elbowPosition=Elbow.position
    (station.intern==:elevator ? 
      elbowPosition.base.kind_of?(Integer) : elbowPosition==Elbow[station]) and
        Arm.extended? elbowPosition
  end
  
  def Arm.ready?
    not (Forearm.status.lost or Elbow.status.lost)
  end
  
  def Arm.asIRBtext
    elbowPosition = Elbow.position
    forearmPosition = Forearm.position
    forearmTxt = "Forearm retracted"
    unless retracted? forearmPosition
      if extended? elbowPosition, forearmPosition
        elbowPosition = :elevator if elbowPosition.base.kind_of? Integer
        return "Arm at #{elbowPosition}, #{Hand.asIRBtext}"
      else
        forearmTxt = forearmPosition.asIRBtext
      end
    end   
    "#{elbowPosition.asIRBtext}, #{forearmTxt}, #{Hand.asIRBtext}"
  end
  
end  #module Arm


class << SE  #up and down are reversed from moveUp and moveDown
  def up puckCount=nil, tmpCfg=nil, maxDuration=@maxDuration
    if puckCount.is_a? Numeric
      moveDown(puckCount, tmpCfg, maxDuration)
    else  #if puckCount not Numeric, assume it is actually tmpCfg
      # effectively -- SE.up tmpCfg=nil, maxDuration=@maxDuration
      tmpCfg = maxDuration unless tmpCfg
      moveTo(:up, puckCount, tmpCfg)
    end   
  end
  def down puckCount=nil, tmpCfg=nil, maxDuration=@maxDuration
    if puckCount.is_a? Numeric
      moveUp(puckCount, tmpCfg, maxDuration)
    else  #if puckCount not Numeric, assume it is actually tmpCfg
      # effectively -- SE.down tmpCfg=nil, maxDuration=@maxDuration
      tmpCfg = maxDuration unless tmpCfg
      moveTo(:down, puckCount, tmpCfg)
    end   
  end
  
  def rawId counts, slop=@slop
  #given raw position in counts, return
  # corresponding position
    if counts > SE.raw(:bottom)-slop or (counts-SE.raw(:top)).abs <= slop
      baseRawId counts, slop #report down as :down, not x.yy pucks
    else
      super
    end
  end

  def position slop=@slop
  #return the named position if top or bottom, else pucks below top plate
    rawId rawPosition, slop
  end

end


class Clamp
  def closeAndVerifyPuckPresence(*args)
  #close the clamp and verify that there is a puck in it
  #note that Clamp.to :close DOES NOT verify the presence of a puck
    closing = log(8){to(:close,*args)} #sample status every 250ms while closing
    minSpeed = config.minSpeed
    previousVelocity = atStart = 0
    springLen = 5
    atEnd = closing.size
    while atStart < atEnd           #search forward just past acceleration
      velocity = closing[atStart].velocity
      break if velocity >= minSpeed and velocity - previousVelocity < 3
      previousVelocity = velocity
      atStart += 1
    end
    raise Error.new "#{name} always accelerating!", self if
      atStart >= atEnd
    atStart-=1 if atStart > 0
    pastStart = atStart+springLen
    nextVelocity = 0
    while (atEnd-=1) >= pastStart #search backward to just before decelleration
      velocity = closing[atEnd].velocity
      break if velocity >= minSpeed and velocity - nextVelocity < 3
      nextVelocity = velocity
    end
    raise Error.new "#{name} always decellerating!", self if
      atEnd < pastStart
    atEnd+=1 if atEnd < closing.size-1
    unsprungCurrent = 0
    closing[atStart,springLen].each {|status| 
      unsprungCurrent+=status.current
    }
    sprungCurrent = 0
    closing[atEnd-springLen+1,springLen].each {|status|
      sprungCurrent+=status.current    
    }
    raise Error.new "No Puck Clamped in #{name}", self if 
      Float(sprungCurrent) / Float(unsprungCurrent) < 1.2
    self
  end
  alias_method :close, :closeAndVerifyPuckPresence
end

class << CC
  def close (*args)
  #closing the collection clamp causes the next sampling
  #to recalibrate pressure sensors and reset sampling speed
    result = closeAndVerifyPuckPresence(*args)
    Sampler.forgetCalibration.inhaleConfig = SSinhale
    result
  end
end  


module FlushPuck

  def self.load
    Log.record "Loading FlushPuck"
    raise "Collection Clamp is already closed!" if CC.closed?
    Arm.grab :flush, :collection
    CC.close
    Arm.retract
  end
  
  def self.unload
    Log.record "Unloading FlushPuck"
    Arm.grab :collection
    CC.open
    Arm.releaseAt :flush
  end
  
  def self.ready?
    not CC.status.lost
  end
  def self.ready!
    CC.home.open
  end
          
end

Flush = FlushPuck


module Storage

  def self.ready?
  #true if storage subsystem is not lost
    Arm.ready? and not(SC.status.lost or SE.status.lost)
  end
  
  def self.ready!
  #sort of a Catch-22.  We need to raise the elevator to home it
  #but, we can't move the carousel if the elevator is not down
  #assume the carousel is aligned and try homing the elevator first
    openPC = Thread(:openPC) {PC.open}  #open clamps so Arm can always move
    CC.open
    openPC.join
    Arm.ready!  #always move arm out of the way first
    
    begin
      SE.home.to :down  #first, try to home the elevator
    rescue Slide::Error =>elevatorErr
      case elevatorErr.reply.error
      when :overCurrent, :positionErr, :trajectoryErr, :speedErr
        Log.recordException elevatorErr
        begin #in case SE resting on an edge of the carousel
          SE.cancelHome.hold.jog (SE.status.home ? SEdownBump : SEupBump)
        rescue Slide::Error
        end
        initialSC = SC.status  #can't use rawPosition as it requires SC be homed
        begin
          SC.home.to 1  #try this even though elevator is probably in the way
        rescue Slide::Error =>initialErr
          case initialErr.reply.error
          when :overCurrent, :positionErr, :trajectoryErr, :speedErr
            Log.recordException initialErr
            bump = SCbump    #opposite direction to center elevator in slot
            bump = -bump if SC.status.position > initialSC.position
            SCbumps.times {  #Try to get interfering elevator out of the way.
              begin
                SC.cancelHome.hold.jog bump
              rescue Slide::Error =>carouselErr
                case carouselErr.reply.error
                when :overCurrent, :positionErr, :trajectoryErr, :speedErr
                  Log.recordException carouselErr
                  bump = -bump  #change direction
                  next
                else
                  raise
                end
              end    #We've managed to move the SC               
              begin  #Try homing the SE again
                SE.home.to :down
              rescue Slide::Error =>elevatorErr
                case elevatorErr.reply.error
                when :overCurrent, :positionErr, :trajectoryErr, :speedErr
                  Log.recordException elevatorErr
                  begin #in case SE resting on an edge of the carousel
                    SE.cancelHome.hold.jog (SE.status.home ? 
                                                SEdownBump : SEupBump)
                  rescue Slide::Error
                  end
                  next  #try moving SC further in the same direction to free SE
                else
                  raise
                end
              end
              return SC.home.to 1  #should work now that elevator is down
            }
            raise "Failed to Ready Storage!"
          else
            raise
          end
        end
        return SE.home.to :down
      else
        raise
      end
    end
    SC.home.to 1  #should work once elevator is out of the way
  end

end


module Processing

  def self.ready?
  #true if processing stage is not lost
    not(PC.status.lost or PS.status.lost)
  end
  
  def self.ready!
    rdyProc = Thread :rdyProcessing do
      PSV.to :air
      PS.home.to 0
      PS.coast
    end
    PC.home.open
    rdyProc.join
  end

end

    
module ESP

  def self.ready?
  #return true if all axes have been homed
    missingAxes = AxisKernel.missing
    raise "Missing #{missingAxes.collect{|a|a.name}.join ', '}" unless missingAxes.empty?
    Sampler.ready? and Storage.ready? and Processing.ready?
  end
  
  def self.ready!
  #(re-)home any lost axes
    rdySampler = Thread(:rdySampler) do
      Sampler.ready! unless Sampler.ready?
    end
    Processing.ready! unless Processing.ready?
    rdySampler.join
    Storage.ready! unless Storage.ready?
    self
  end
  
end


module Sleep

  def self.for duration
  #shutdown everything except the host for specified delay
    nap {delay duration}
  end
  
  def self.until time
  #shutdown until specified time
    wakeTime=nil
    nap {wakeTime = delayUntil time}
    wakeTime
  end
  
  def self.nap
    shutdown
    yield
    wakeup
  end
  
  def self.shutdown
  #shut down all motors and dwarves (leaves host and radio on for now)
    Sleepy.power :camera, :off
    Sleepy.power :raw, :off
    Sleepy.power :dwarf, :off
    Slide.each {|s| s.forget}
    Valve.forget
  end
  
  def self.restart
  #restart after having been shut down
  #dwarf 32kHz osc. may not start up, so cycle power a few times if need be
    triesRemaining = 5
    begin
      Sleepy.restart  #turns on dwarf power and sequences reset
      Solenoid::Valves.reconfigureAll  #reprogram the valve pulse durations
      Shaft.each {|s| s.configure}
      Axis.each {|rollCall| rollCall.status}
    rescue =>err
      Sleepy.power :dwarf, :off
      raise if (triesRemaining-=1) <= 0 
      Thread.log.recordException err
      Delay.sleep 2
      retry
    end      
    Sleepy.power :raw, :on
    Delay.sleep 0.25  #wait for caps to charge
    Valve.reset
  end

  def self.wakeup
  #restart after having been shut down
    restart
    ESP.ready!
  end

end


class << PuckCamera
  def imageName assay=:assay, volume=Sampler.sampleVolume,
      stamp=Thread.schedule.time
  #return a standard name for an image for the specifed assay
     volume=volume.round if volume.respond_to? :round
     assay.to_s + "%02d"%(stamp.year % 100) <<
       ParseDate::MONTHS.index(stamp.month) << 
       "%02d"%stamp.day << "%02d"%stamp.hour << "h#{volume}ml"