#--
#******************************************************************************
#* Copyright 1990-2007 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
#******************************************************************************

require 'ezservo'

# Motor designations used in commands match
# ezservo active_motor_num designations
#
$STBD_RACK = "1"
$PORT_RACK = "2"
  
# 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
  #
  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

  # Initializer used to create rack object in main rover object.
  # Do not assume that rack controller has its' power on 
  #
  def Rack.rover
    r = Rack.new nil, '/dev/rack'
    `stty -F /dev/rack min 1 time 0`
    r
  end 

  # Initialize/Reinitialize controller. Must pass in which rack to
  # operate on.
  #
  def reinit(rack_id)
    return nil unless rack_id == $STBD_RACK || rack_id == $PORT_RACK

    # Cycle power to the rack controller before setting controller
    # parameters. Delays are used to allow controller to initialize
    # communications.
    #
    if (rack_id == $STBD_RACK)
      Rover.instance.relay.s_rackOff
      sleep(0.5)
      Rover.instance.relay.s_rackOn
    else
      Rover.instance.relay.p_rackOff
      sleep(0.5)
      Rover.instance.relay.p_rackOn
    end
    set_active_rack(rack_id)
    sleep(0.5)

    # 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")
    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 the controller responds with "bad operand")
    send_message("s2R")   # store null program for use in finding insert switch
    send_message("V3000R")
  end

  # Initialize for testing and experimentation - does not set limit
  # switches so do not use to insert or home chamber.
  #
  def reinit_basic(rack_id)
    return nil unless set_active_rack(rack_id)
    
    if (rack_id == $STBD_RACK)
      Rover.instance.relay.s_rackOff
      sleep(0.5)
      Rover.instance.relay.s_rackOn
    else
      Rover.instance.relay.p_rackOff
      sleep(0.5)
      Rover.instance.relay.p_rackOn
    end
    sleep(0.5)

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

  # Usage: set_active_rack($STBD_RACK)
  # Make sure controller is talking to correct rack motor.
  #
  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)
  # Tell the controller to home the chamber using the home limit switch
  #
  def home(rack_id)
    return false unless set_active_rack(rack_id)
    self.send_message("Z30000R")
    begin
      return self.wait_for_move_to_complete(1)
    rescue ServoOverloadError => e
      self.halt
      syslog("! Servo Overload Error. Halting move at pos #{e.servo_pos}")
      return false
    end
  end

 
  # Usage: insert_chamber($STBD_RACK)
  # Tell the controller to insert the chamber using the insertion limit switch
  #
  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
  # allowing for multiple attempts.
  #
  def move_home(rack_id, retry_count = NUM_RACK_RETRIES)
    move_rack(rack_id, :home, retry_count)
  end
  
  # Insert the rack using the generalized move method
  # allowing for multiple attempts.
  # 
  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 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


class SimRack < SimEzservo
  include LogHelper

  def initialize config, controller_id='1', portname='/dev/rack'
    syslog("_ intializing sim rack to portname #{portname}")
  end
  
  def SimRack.rover
    r = SimRack.new nil, '/dev/rack'
  end 

  def reinit(rack_id)
    if (set_active_rack(rack_id))
      syslog("_ rack #{rack_id} re-inited")
    else
      syslog("_ rack #{rack_id} NOT re-inited")
    end
    return "sim command"
  end
  
  def home(rack_id)
    syslog("_ sending rack #{rack_id} home")
    sleep(2)
    true
  end

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

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

  def move_to_insert(rack_id, retry_count = 1)
    sleep(2)
    syslog("_ Rack moved by move_to_insert(#{rack_id}) after 1 attempts")
    true
  end

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

  def move_home(rack_id, retry_count = 1)
    sleep(2)
    syslog("_ Rack moved by move_home(#{rack_id}) after 1 attempts")
    true
  end

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

  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

  def encoder_position
    return 2000
  end
end