######################## rack.rb  henthorn@mbari.org ###########################
#
# May07 - Parameterized with rack controller id (RS-485)
# May07 - Modified move_rack - no longer controlling relays
#
################################################################################

# Motor designations used in commands match
# ezservo active_motor_num designations
#
$STBD_RACK = "1"
$PORT_RACK = "2"
  

require 'ezservo'

class Rack < Ezservo
  
  NUM_RACK_RETRIES = 3          #number of attempts to make at performing a rack
                                #insertion or home

  def initialize config, portname='/dev/rack' 
    unless config
      config = SerialPort::Configuration.default
      #default baud is 38.4k, let's change it to 9600
      config.baud = 9600
    end
    super(config, portname)
    
    @port_name = portname
    self.start_listener
    set_active_rack($STBD_RACK)
  end

  def Rack.rover
    r = Rack.new nil, '/dev/rack'
    `stty -F /dev/rack min 1 time 0`
    r
  end 

  def reinit(rack_id)
    return nil unless rack_id == $STBD_RACK || rack_id == $PORT_RACK
    
    if (rack_id == $STBD_RACK)
      Rover.instance.relay.s_rackOff
      sleep(1)
      Rover.instance.relay.s_rackOn
    else
      Rover.instance.relay.p_rackOff
      sleep(1)
      Rover.instance.relay.p_rackOn
    end
    
    set_active_rack(rack_id)
    sleep(1)

    #  Set H-E limit switches
    #first send a message to flush the buffer
    #because there could be junk in it if the rack was not
    #on when this object was first initialized
    send_message("badR")
    send_message("f1R")   # set home flag and limit polarity
    send_message("n2R")   # enable limit switches
    send_message("f1R")   # set home flag and limit polarity, again (first time ezservo
                          # responds with "bad operand")
    send_message("s2R")   # store null program for use in finding insert switch
    send_message("V3000R")
  end

  def reinit_basic(rack_id)
    return nil unless set_active_rack(rack_id)
    
    if (rack_id == $STBD_RACK)
      Rover.instance.relay.s_rackOff
      sleep(1)
      Rover.instance.relay.s_rackOn
    else
      Rover.instance.relay.p_rackOff
      sleep(1)
      Rover.instance.relay.p_rackOn
    end
    sleep(1)

    send_message("badR")
    send_message("V3000R")
  end

  # Usage: set_active_rack($STBD_RACK)
  #
  def set_active_rack(rack_id)
    return false if rack_id != $PORT_RACK && rack_id != $STBD_RACK
    self.active_motor_num = rack_id
    syslog("_ Rack: active rack set to #{rack_id}")
    true
  end

  # Set motor speed of a rack motor
  #
  def set_rack_speed(rack_id, counts_per_sec)
    return false unless set_active_rack(rack_id)
    msg = "V" + #{counts_per_sec}.to_s + "R"
    syslog("_ Setting rack speed: #{msg}")
    self.send_message(msg)
  end
  
  # Usage: home($STBD_RACK)
  #
  def home(rack_id)
    return false unless set_active_rack(rack_id)
    self.send_message("Z30000R")
    return self.wait_for_move_to_complete(1)
  end

 
  # Usage: insert_chamber($STBD_RACK)
  #
  def insert_chamber(rack_id)
    set_active_rack(rack_id)
    self.send_message("gP100S11e2G0R")
    if (self.wait_for_move_to_complete(1))
      self.stop
      return true
    else
      return false
    end
  end

  # Home the rack using the generalized move method
  # 
  def move_home(rack_id, retry_count = NUM_RACK_RETRIES)
    move_rack(rack_id, :home, retry_count)
  end
  
  # Home the rack using the generalized move method
  # 
  def move_to_insert(rack_id, retry_count = NUM_RACK_RETRIES)
    move_rack(rack_id, :insert_chamber, retry_count)
  end
  
  # Generalized move method. Caller passes in the Rack class method
  # to be used, the rack id, and how many attempts to make
  # 
  def move_rack(rack_id, method, retry_count)
  
    move_complete = false
    
    # Start the attempts
    # 
    for i in 1..retry_count
      syslog("_ Attempt \##{i} to move rack by #{method}(#{rack_id})")
      if(!self.send(method, rack_id.to_s))
        syslog("! failed attempt to move rack by #{method}")
      else
        syslog("_ Rack successfully moved by #{method}(#{rack_id}) after #{i} attempts")
        move_complete = true
        break
      end
    end

    if(!move_complete)
      syslog ("! Unable to move rack by #{method}(#{rack_id})")
    end

    return move_complete
  end

  # Query the controller switches, return the value
  # of the "insert" switch (4th one in the string
  # which corresponds to the switch #1)
  # The caller interprets the value (whether on or off)
  #
  def insert_query(rack_id)
    val = switch_query(rack_id, 4)
  end
  
  # Query the controller switches, return the value
  # of the "home" switch (2nd one in the string
  # which corresponds to the switch #3)
  # The caller interprets the value (whether on or off)
  #
  def home_query(rack_id)
    switch_query(rack_id, 2)
  end
  
  # Query the controller switches, return the value
  # of the requested switch (1st one in the string
  # corresponds to the switch #4 - it's backward)
  #
  def switch_query(rack_id, switch)
  
    self.set_active_rack(rack_id)
    ret = nil
    if (switch < 1 || switch > 4)
      syslog("! Invalid switch passed to switch_query (#{switch}")
      return ret
    end
    
    # We have to stop the listener thread
    # because it will intercept the response
    # and do special interpretation
    #
    self.kill_listener
    sleep(2)

    # Send the command to the controller and get the response
    # 
    query = "/#{rack_id}?aa"
    @serial_port.write(query+"\r")
    resp = @serial_port.gets
    syslog("_ switch_query(#{query}) returned \"#{resp}\"")

    # The response should contain 4 values seperated by commas,
    # so 3 commas 
    if (resp && (resp.count(",") == 3))
      resp = resp[4..resp.size]       # strip off the control characters
      resp_ary = resp.split(",")      # split the values into an array
      if (4 == resp_ary.size)         # there better be 4 elements
        switch -= 1
        ret = resp_ary[switch] # get the requested element
      end
    end

    # Restart the listener thread
    #
    self.start_listener
    
    if (0 == ret.to_i)
      syslog("! switch value not an integer? (#{ret})")
      ret = nil
    else
      ret = ret.to_i
    end
    
    return ret
  end
  
end