=begin
 ******************************************************************************
 * Copyright 1990-2009 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Implemetation of the HomeChambers behavior
 * Filename : home_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'

##########
# HomeChambers class.
# Home both chambers to the home position.
#
class HomeChambers < Behavior
  include RoverTime

  #####
  # 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 HomeChambers.usage
    u  = "\n<HomeChambers name=\"Example\" comment=\"Home 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

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

      # Home starboard chamber first
      # Initialize the rack, open the valves, home the chamber.
      #
      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 home
        Rover.instance.relay.s_rackOn
        if (Rover.instance.stbd_rack.reinit)
          Rover.instance.relay.s_valveOn

          # This call uses a thread to monitor the progress of the rack
          #
          unless (Rover.instance.stbd_rack.move_home)
            syslog("! Unable to home stbd chamber")
          end
          Rover.instance.relay.s_valveOff

        else
          syslog("! Unable to intialize stbd chamber")
        end
        Rover.instance.relay.s_rackOff
      else
        syslog("_ bad controller so no go")
      end

      # Repeat for the port chamber
      #
      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 home
        Rover.instance.relay.p_rackOn
        if (Rover.instance.port_rack.reinit)
          Rover.instance.relay.p_valveOn

          # This call uses a thread to monitor the progress of the rack
          #
          unless (Rover.instance.port_rack.move_home)
            syslog("! Unable to home port chamber")
          end
          Rover.instance.relay.p_valveOff

        else
          syslog("! Unable to intialize port chamber")
        end
        Rover.instance.relay.p_rackOff
      else
        syslog("_ bad controller so no go")
      end
      
      syslog("_ #{@name} complete")
    ensure
      self.clean_up()
    end
  end
  #####


  #####
  def clean_up
    Rover.instance.relay.s_rackOff
    Rover.instance.relay.p_rackOff
    Rover.instance.relay.s_valveOff
    Rover.instance.relay.p_valveOff
  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 HomeChambers.test
    require 'rexml/document'
    require 'device/rover'

    Rover.instance
    # Test code here
    home_xml = REXML::Document.new(File.new(Misc.find_plan("test_home.xml")))
    lc = HomeChambers.new(home_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(HomeChambers.usage)
    exit(0)
  end

  HomeChambers.test
end