=begin
 ******************************************************************************
 * Copyright 1990-2009 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Implemetation of the InsertChambers behavior
 * Filename : insert_chambers.rb
 * Author   : Henthorn
 * Project  : Benthic Rover
 * Version  : 1
 * Created  : Dec 08
 * Modified : 
 ******************************************************************************
=end

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

require "utils/misc"
require "control/behavior"
require 'device/rover'

##########
# InsertChambers class.
# Insert both chambers to the inserted position.
#
class InsertChambers < Behavior
  include RoverTime

  ##
  # This constant is used to configure rack drivers with bad insert switches
  #
  MaxInsertPos = 11200
  
  #####
  # Accept a behavior element and extract parameters
  #
  def initialize(p_behavior_element)
    super

    load_attrs(p_behavior_element)
  end
  #####


  #####
  # A crude documentation feature. Each subclass should provide
  # one that begins by calling this method.
  #
  def InsertChambers.usage
    u  = sprintf(Behavior.usage_format, \
                 "prefix","Prepends to image filenames","String","blank")
    u = "\n<InsertChambers name=\"Example\" comment=\"Insert respirometer chambers\" />\n"
  end


  #####
  # Iterate over attributes in the element, extract parameters.
  #
  def load_attrs(p_behavior_element)
    super
    return

    # Keep for later use
    #
    attrs = p_behavior_element.attributes
    attrs.keys.each do |k|
      case k

        # Total duration of the launch
        when "duration"
        @duration = attrs[k].to_f

#       else
#       puts("Unrecognized attr: #{k}")

      end
    end

    # Option: Throw an exception if there any required attributes
    # missing
    #
  end
  #####


  #####
  # Initialize behavior and make ready for execution.
  # Should be called just before executing the behavior.
  #
  def init(p_seq_number=1)   # Where in the plan behavior sequence I am
    syslog("_ #{@name} init")
    self.data_home = p_seq_number

    # Perform initialization procedures
    @state = READY
  end
  #####


  #####
  # Execute the behavior
  #
  def execute
    # Execute if ready.
    unless (@state == READY)
      syslog("! #{name} not in ready state")
      return
    end

    syslog("_ #{@name} executing")
    @state = EXECUTING

    # Perform the behavior
    begin
      relay = Rover.instance.relay

      # Let's watch when we're on MARS
      #
      if (Supervisor.instance.deployment.mars)
        relay.camerasOn
        relay.videoOn
      end

      ##
      # Sample oxygen data during insertion
      #
      if (false)
        Rover.instance.stbd_ref_optode.run
        Rover.instance.port_optode.run
        Rover.instance.starboard_optode.run
        relay.optodesOn
      end
      
      # Insert starboard chamber first
      # Initialize the rack, open the valves, insert the chamber.
      # Skip insertion if the health of the rack controller is bad.
      #
      h = nil
      s = Supervisor.instance.dbif.load_component_state(RoverStore::SRack)
      h = s[RoverStore::Health] if (s)
      if (h == nil || h == RoverStore::InsertSwitch)  # Ok to insert

        Rover.instance.relay.s_rackOn
        if (Rover.instance.stbd_rack.reinit)
          relay.s_valveOn

          # This routine uses a thread to monitor the rack motor
          #
          rack = Rover.instance.stbd_rack
          rack.max_position_on_insert = MaxInsertPos if (h)

          unless (rack.move_to_insert)
            syslog("! Unable to insert stbd chamber")
            if (rack.low_switch?)
              syslog("_ at low limit switch")
              s = Supervisor.instance.dbif.load_component_state(RoverStore::SRack)
              s[RoverStore::Health] = RoverStore::InsertSwitch
              Supervisor.instance.dbif.save_component_state(RoverStore::SRack, s)
            end
          end
          relay.s_valveOff

        else
          syslog("! Unable to intialize stbd chamber")
        end
        relay.s_rackOff

      else
        syslog("_ bad controller so no go")
      end

      # Same for the port chamber
      # Skip insertion if the health of the rack controller is bad.
      #
      h = nil
      s = Supervisor.instance.dbif.load_component_state(RoverStore::PRack)
      h = s[RoverStore::Health] if (s)
      if (h == nil || h == RoverStore::InsertSwitch)  # Ok to insert

        relay.p_rackOn
        if (Rover.instance.port_rack.reinit)
          relay.p_valveOn

          rack = Rover.instance.port_rack
          rack.max_position_on_insert = MaxInsertPos if (h)

          unless (rack.move_to_insert)
            syslog("! Unable to insert port chamber")
            if (rack.low_switch?)
              syslog("_ at low limit switch")
              s = Supervisor.instance.dbif.load_component_state(RoverStore::PRack)
              s[RoverStore::Health] = RoverStore::InsertSwitch
              Supervisor.instance.dbif.save_component_state(RoverStore::PRack, s)
            end
          end
          relay.p_valveOff
        else
          syslog("! Unable to intialize port chamber")
        end
        relay.p_rackOff

      else
        syslog("_ bad controller so no go")
      end

      ##
      # Turn off optode data acquisition
      #
      if (false)
        Rover.instance.stbd_ref_optode.stop
        Rover.instance.port_optode.stop
        Rover.instance.starboard_optode.stop
        Rover.instance.relay.optodesOff
      end
      
      syslog("_ #{@name} complete")
    ensure
      self.clean_up()
    end
  end
  #####


  #####
  def clean_up
    Rover.instance.relay.s_valveOff
    Rover.instance.relay.p_valveOff
    Rover.instance.relay.s_rackOff
    Rover.instance.relay.p_rackOff
    if (false)
      Rover.instance.relay.optodesOff
      Rover.instance.stbd_ref_optode.stop
      Rover.instance.port_optode.stop
      Rover.instance.starboard_optode.stop
      Rover.instance.relay.optodesOff
    end
  end
  #####


  #####
  # Abort the behavior
  #
  def abort
    syslog("_ #{@name} abort")
    if (@state == EXECUTING)
      @state = ABORTING 

      # Kill the run thread, clean up
      @my_thread.kill if @my_thread
      clean_up
    else
      syslog("_ #{@name} not executing, aborting...")
    end
  end
  #####


  #####
  # Test method. Use to run standard test of MyClass from
  # a ruby script.
  # 
  def InsertChambers.test
    require 'rexml/document'
    require 'device/rover'

    Rover.instance
    # Test code here
    insert_xml = REXML::Document.new(File.new(Misc.find_plan("test_insert.xml")))
    lc = InsertChambers.new(insert_xml.root)
    lc.init
    lc.execute
    puts("Done")
  end
  #####

end

#####
# Standalone unit test (recommended)
# Include at the end of the file as a hook to run a unit test of
# the code from the command line including the command line options
# (e.g., "$ ruby my_class.rb --sim").
#
if __FILE__ == $0
  if "usage" == ARGV[0]
    puts(Behavior.usage)
    puts(InsertChambers.usage)
    exit(0)
  end

  InsertChambers.test
end