=begin
 ******************************************************************************
 * Copyright 1990-2010 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Implemetation of the Move Carousel behavior
 * Filename : collection_to_fluor.rb
 * Author   : Virrey
 * Project  : Sedimentaion Event Sensor
 * Version  : 1
 * Created  : Aug 10
 * 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 CollectionToFluor < Behavior

  $HOME_POS = 0
  $COLLECTION_POS = 6720
  $FLUOR_POS = 29280
  $CAMERA_POS = 35040
  
  #####
  # Accept a behavior element and extract parameters
  #
  def initialize(p_behavior_element)
    super

    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

#       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

      # Assume the power is on the motor controller.
      # For now, create easy stepper object here.
      # 
      ez = Ezstepper.ses
      
      here = ez.encoder_position[0]
      if (here.to_i != $COLLECTION_POS)
        syslog("! Not at collection position: #{here}")
        return
      end
      
      # If the position of the stepper is equal to the desired position on the
      # turntable, the stepper will  proceed to the next location
      #
      ez.forward(($FLUOR_POS-$COLLECTION_POS))
      
      # Takes the position read by the motor controller and stores the value to 
      # the start up program of the controller
      #
      here = ez.encoder_position[0]
      ez.send_message("s0z#{here}R")

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

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


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

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