=begin
 ******************************************************************************
 * Copyright 1990-2010 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Implemetation of the Move Carousel behavior
 * Filename : move_carousel.rb
 * Author   : Henthorn
 * Project  : Sedimentaion Event Sensor
 * Version  : 1
 * Created  : Aug 11
 * Modified : 
 ******************************************************************************
=end

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

require "utils/misc"
require "control/behavior"
require "control/supervisor"
require "device/ezstepper"
# require 'device/SES'

##########
# Move Carousel class.
# Move carousel to a desired position
#
#
class MoveCarousel < Behavior

  #####
  # Accept a behavior element and extract parameters
  #
  def initialize(p_behavior_element)
    super

    @steps = $HOME_POS
    
    load_attrs(p_behavior_element)
  end
  #####


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

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

      when "steps"
        @steps = attrs[k].to_i
      
      when "location"
        loc = attrs[k].downcase
        
        case loc
        
        when "collection"
          @steps = $COLLECTION_POS
      
        when "camera"
          @steps = $CAMERA_POS
      
        when "fluoro"
          @steps = $FLUOR_POS
      
        else
          puts("Unrecognized location: #{loc}")
          
        end
      
      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")
    
    # 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

      # Crank up the stepper motor controller and reinit
      #
      Rover.instance.relay.stepperOn
      sleep(3)
      ez = Rover.instance.motors
      ez.reinit

      ez.move_abs(@steps)
      
      ez.save_position_for_startup(@steps)

      syslog("_ #{@name} complete")

    ensure
      self.clean_up()
    end
  end
  #####


  #####
  def clean_up
    Rover.instance.relay.stepperOff

  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 MoveCarousel.test
    require 'rexml/document'
    # require 'device/ses'

    # Test code here
    move_xml = REXML::Document.new(File.new(Misc.find_plan("test_carousel.xml")))
    lc = MoveCarousel.new(move_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
  MoveCarousel.test
end