=begin
 ******************************************************************************
 * Copyright 1990-2009 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Benthic resp chamber rack assembly abstraction/interface
 * Filename : rack.rb
 * Author   : Henthorn
 * Project  : Benthic Rover
 * Version  : 2
 * Created  : Oct-06  Initial functionality
 * Modified : May-07  Parameterized methods for dual rack assembly
 *          : Nov-08  Updated to lib.2 (barbo)
 *          : Aug-10  Rack id is defined when the object is created using
 *          :         Rack.rover_port and Rack.rover_stbd. 
 ******************************************************************************
=end

# Refer to Rover-code home directory as base
#
require "#{ENV['ROVER_HOME']}/utils/rover_environment"
#

require 'device/ezservo'

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

$INSERT_SPEED = "3000"
$HOME_SPEED = "2300"
$CURR_LIMIT = "50"
  
# Abstract representation of Rover rack controller
# FIXME: Probably should not derive from Ezservo, which is really
# the propulsion motor controller astraction. Just use an Ezservo
# object within Rack.
#
class Rack < Ezservo
  
  NUM_RACK_RETRIES = 3    # number of attempts to make at performing a rack
                          # insertion or home

  # Standard initializer. Assumes stbd rack. Use set_active_rack($PORT_RACK)
  # to change it.
  #
  def initialize(portname='/dev/rack') 
    super(portname)

    @port_name = portname
    self.active_motor_num = $STBD_RACK
    @max_pos = nil
    @at_max_pos = false

    self.start_listener
    self.overload_timeout=25000
  end

  # Initializers used to create stbd and port rack objects in main rover object.
  # Do not assume that rack controller has its' power on 
  #
  def Rack.rover_stbd
    r = Rack.new('/dev/stbd_rack')
    `stty -F /dev/stbd_rack min 1 time 0`
    r.set_active_rack($STBD_RACK)
    r
  end 

  def Rack.rover_port
    r = Rack.new('/dev/port_rack')
    `stty -F /dev/port_rack min 1 time 0`
    r.set_active_rack($PORT_RACK)
    r
  end 

  def valid_rack_id(rack_id)
    return false unless (rack_id == $STBD_RACK || rack_id == $PORT_RACK)
    true
  end

  # Initialize/Reinitialize controller. Must pass in which rack to
  # operate on.
  # Power to the rack controller set by the caller.
  #
  def reinit

    @at_max_pos = false
    sleep(0.2)

    # Set H-E limit switches (for chamber homing and insertion).
    # 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")
    sleep(0.1)
    return nil unless (send_message("f1R"))  # set home flag and limit polarity
    sleep(0.1)
    return nil unless (send_message("n2R"))  # enable limit switches
    sleep(0.1)
    return nil unless (send_message("f1R"))  # set home flag and limit polarity
    sleep(0.1)
    return nil unless (send_message("s2R"))  # store null program
    sleep(0.1)
    return nil unless (send_message("V3000R"))
    sleep(0.1)

    pos = self.encoder_position
    syslog("_ rack #{self.active_motor_num} at position #{pos}")
  end

  # Initialize for testing and experimentation - does not set limit
  # switches so do not use to insert or home chamber.
  # Power to the rack controller set by the caller.
  #
  def reinit_basic
    sleep(0.2)

    send_message("badR")
    sleep(0.1)
    send_message("V3000R")
    sleep(0.1)
  end

  ##
  # Save the current position in the controller for
  # initialization on next power-up.
  # Return nil if there is a comms problem.
  #
  def save_current_position_for_startup
    pos = self.encoder_position
    if (has_error? || @timed_out)
      syslog("! unknown position")
      return nil
    end
    send_message("s0z#{pos}R")
    pos
  end
  
  # Usage: set_active_rack($STBD_RACK)
  # Make sure controller is talking to correct rack motor.
  #
  def set_active_rack(rack_id)
    return false unless (valid_rack_id(rack_id))
    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(counts_per_sec)
    msg = "V" + counts_per_sec.to_s + "R"
    syslog("_ Setting rack speed: #{msg}")
    self.send_message(msg)
  end
  
  # Set motor current limit
  # Percentage 1-100
  #
  def set_rack_current_limit(percentage)
    msg = "m" + percentage.to_s + "R"
    syslog("_ Setting rack current limit: #{msg}")
    self.send_message(msg)
  end
  
  # Usage: home
  # Tell the controller to home the chamber using the home limit switch
  #
  def home()
    return true if (home_switch?)
    
    return false unless set_rack_current_limit("99")
    return false unless set_rack_speed($HOME_SPEED)
    return false unless self.send_message("Z20000R")

    ret = false
    begin
      if (self.wait_for_move_to_complete(1))
        ret = true
      end
    rescue Exception
      e = $!
      self.halt
      syslog("! Servo Overload Error? Halting. #{e}")
    end
    save_current_position_for_startup()
    return ret
  end

  def max_position_on_insert=(val)
    @max_pos = val
    syslog("_ max position on insert set to #{val}")
  end
 
  # Usage: insert_chamber
  # Tell the controller to insert the chamber using the insertion limit switch
  #
  def insert_chamber()
    return true if (insert_switch?)

    return false unless set_rack_current_limit($CURR_LIMIT)
    return false unless set_rack_speed($INSERT_SPEED)
    return false unless self.send_message("gP100S11e2G0R")

    ##
    # Using this method provides more assurance
    # that the racks do not insert past the desired
    # insertion position/
    #
    ret = false
    if (self.wait_for_insert_to_complete(@max_pos))
      ret = true
    end
    self.stop
    save_current_position_for_startup()
    return ret
  end

  # Home the rack using the generalized move method
  # allowing for multiple attempts.
  #
  def move_home(retry_count = NUM_RACK_RETRIES)
    for i in 1..retry_count
      return true if home_switch?

      syslog("_ Attempt \##{i} :home (#{@active_motor_num})")
      move_rack(:home)

      return true if home_switch?

      # Re-init rack and try again
      self.reinit
    end

    return false
  end
  
  # Insert the rack using the generalized move method
  # allowing for multiple attempts.
  # 
  def move_to_insert(retry_count = NUM_RACK_RETRIES)

    for i in 1..retry_count
      syslog("_ Attempt \##{i}  :insert_chamber (#{@active_motor_num})")
      move_rack(:insert_chamber)

      ##
      # Move is finished. See if we're at the insert switch, the
      # low switch, or at the alternative max insert position.
      #
      if (insert_switch? || @at_max_pos)
        return true
      elsif (low_switch?)
        return false
      end

      # Re-init rack and try again
      self.reinit
    end

    return false
  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(method)
    move_complete = false
    if(!self.send(method))
      syslog("! failed attempt to move rack by #{method}")
    else
      syslog("_ Rack completed move by #{method}(#{@active_motor_num})")
      move_complete = true
    end

    if(!move_complete)
      syslog("! Unable to move rack by #{method}(#{@active_motor_num})")
    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
    val = switch_query(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
    switch_query(2)
  end

  # Query the insert switch for the rack.
  # Return true if switch is activated.
  #
  def insert_switch?
    nval = insert_query()
    return false unless (nval)

    val = (nval < 2000)? true : false
    syslog("_ insert_switch? #{val}")
    return val
  end

  # Query the home switch for the rack.
  # Return true if switch is activated.
  #
  def home_switch?
    nval = home_query
    return false unless (nval)

    val = (nval < 2000)? true : false
    syslog("_ home_switch? #{val}")
    return val
  end

  # Query the lower limit switch for the rack.
  # Return true if switch is activated.
  #
  def low_switch?
    nval = switch_query(1)
    return false unless (nval)

    val = (nval < 2000)? true : false
    syslog("_ low_switch? #{val}")
    return val
  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(switch)
  
    ret = nil
    if (switch < 1 || switch > 4)
      syslog("! Invalid switch passed to switch_query (#{switch}")
      return ret
    end
    
    # Send the command to the controller and get the response
    # 
    max_tries = 3
    for iw in 1..max_tries do
      begin
    
      # We have to stop the listener thread
      # because it will intercept the response
      # and do special interpretation
      #
      self.kill_listener
      sleep(0.5)

      @serial_port.purge
      query = "/#{active_motor_num}?aa\r"
      
      # Must not use send_message method here. Can't have listener running.
      #
      @serial_port.write(query)
      resp = @serial_port.gets
      syslog("_ switch_query(#{query}) returned \"#{resp}\"")

#      @serial_port.seek(-1)   # for testing error handler
      
      # 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

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

      rescue
        syslog("! #{$!} on #{iw} of #{max_tries}")
        if (iw < max_tries)
          self.port_reset
          next
        else
          raise #{$!}
        end
      ensure
      end # max_tries
      
    end
    
  end
  
  # Keep checking motor positions until two consecutive values are equal.
  #
  def wait_for_insert_to_complete(max_position_count=nil)
    self.stop_wait = false
    syslog("_ initializing wait_for_insert_to_complete")
    
    positions = self.encoder_position
    sleep(1.5)
    new_positions = self.encoder_position
    
    update = "position: "
    start_pos = 0
    new_positions.each do |pos|
      update << pos.to_s << ", "
      start_pos = pos
    end

    # Keep checking until the positions are equal or until we abort out of
    # wait cycle.
    #
    while (positions != new_positions && !self.stop_wait) do
      sleep(1)
      
      # Flag and handle error conditions
      #
      if ( (self.has_error? && ("overload error" == self.error_message)) )
        syslog("! error overload detected while waiting for move to complete")
        self.halt    # Perhaps the decision to halt should be up to the caller?
        return false
      end

      positions = new_positions
      new_positions = self.encoder_position
      update = "motors: #{self.active_motor_num} positions: "
      new_pos = start_pos
      new_positions.each do |pos|
        update << pos.to_s << ", "
      	new_pos = pos
	if (max_position_count && pos >= max_position_count)
	  self.halt
	  @at_max_pos = true   # Signal that we're at the max position
	end
      end
      syslog("_ new: " + update)

    end
    
    if (self.stop_wait)
      syslog("_ wait was stopped")
    else
      syslog("_ positions are equal")
    end

    # Flag error condition
    if self.has_error?
      syslog("! error detected while waiting for move to complete")
      syslog("! error: #{self.error_message}")
      return false
    end

    self.stop_wait = false
    syslog("_ move has completed")
    
    return true
  end
  
end


class SimRack < SimEzservo
  def initialize(portname='/dev/rack')
    syslog("_ intializing sim rack to portname #{portname}")
  end
  
  def SimRack.rover_stbd
    r = SimRack.new('/dev/stbd_rack')
  end 

  def SimRack.rover_port
    r = SimRack.new('/dev/port_rack')
  end 

  def reinit
    syslog("_ rack #{@active_motor_num} re-inited")
    return true
  end
  
  def home
    syslog("_ sending rack #{@active_motor_num} home")
    sleep(2)
    true
  end

  def insert_chamber
    syslog("_ inserting rack #{@active_motor_num} chamber")
    sleep(2)
    true
  end

  def insert_query
    ret = 16000   # Ezservo returns value > 16000 if switch is activated
    syslog("_ insert_query returning #{ret}")
    ret
  end

  def move_to_insert(retry_count = 1)
    sleep(2)
    syslog("_ Rack moved to insert(#{@active_motor_num}) after 1 attempts")
    true
  end

  def home_query
    ret = 16000   # Ezservo returns value > 16000 if switch is activated
    syslog("_ home_query returning #{ret}")
    ret
  end

  def home_switch?
    true
  end

  def insert_switch?
    true
  end

  def low_switch?
    true
  end

  def move_home(retry_count = 1)
    sleep(2)
    syslog("_ Rack homed(#{@active_motor_num}) after 1 attempts")
    true
  end

  def set_rack_speed(counts_per_sec)
    syslog("_ Setting rack speed to #{counts_per_sec}")
    return "sim command"
  end

  def encoder_position
    return 2000
  end
end