# 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 : TightForearm
    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?
    Forearm.to Forearm[SC.position.base, forearmOffset], TightForearm
  end
  
  def Arm.ready!
    Forearm.home.to :retract, QuickForearm
    Thread(:ClosingHand) {Hand.close}
    Elbow.home.to :home
    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 botom, else pucks below top plate
    rawId rawPosition, slop
  end

end


class << CC
  def close (*args)
  #closing the collection clamp causes the next sampling
  #to recalibrate pressure sensors and reset sampling speed
    result = to (:close, *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?
    Arm.ready? and not CC.status.lost
  end
  def self.ready!
    Arm.ready! unless Arm.ready?
    CC.home.open
  end
          
end

Flush = FlushPuck


module Storage

  def self.ready?
  #true if storage subsystem is not lost
    not(SC.status.lost or SE.status.lost)
  end
  
  def self.ready!
    SE.home.to :down
    SC.home.to 1
  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
    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
    rdyStorage = Thread(:rdyStorage) do
      Storage.ready! unless Storage.ready?
    end
    Processing.ready! unless Processing.ready?
    rdySampler.join
    rdyStorage.join
    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.wakeup
  #restart after having been shut down
    Sleepy.restart  #turns on dwarf power and sequences reset
    Sleepy.power :raw, :on 
    Valve.reset
    Shaft.each {|s| s.configure}
    ESP.ready!
  end

end