=begin
 ******************************************************************************
 * Copyright 1990-2009 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Implementation of the Rover MoveForward class
 * Filename : move_forward.rb
 * Author   : Henthorn
 * Project  : Benthic Rover
 * Version  : 1
 * Created  : May 2009
 * 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"

##########
# MoveForward behavior
#
class MoveForward < Behavior
  include SyslogWriter
  include RoverTime

  ##
  # Attributes
  #
  attr_reader :name, :distance, :take_images, :image_interval

  #####
  # Construct a MoveForward object given from a xml element
  #
  def initialize(p_behavior_element)
    super

    @exception = false         # Throw an exception during execute
    @distance = nil            # Must be specified in the MakeHeading element
    @take_images = true        # Capture images during move if true
    @image_interval = 1        # Distance between images
    @name = self.class.name
    @prefix = ""
    @sleep = 0                 # Minutes to sleep after the move
    load_attrs(p_behavior_element)

  end
  #####


  #####
  # Iterate over attributes in the element, extract parameters.
  #
  def load_attrs(p_behavior_element)
    attrs = p_behavior_element.attributes
    attrs.keys.each do |k|
      case k

      when "name"
      @name = attrs[k].to_s

      when "distance"
      @distance = attrs[k].to_f

      when "take_images"
      @take_images = attrs[k].to_s.downcase == "true"

      when "image_interval"
      @image_interval = attrs[k].to_f

      when "prefix"       # image prefix
      @prefix = attrs[k].to_s

      when "sleep"        # time (minutes) to sleep after move
      @sleep = attrs[k].to_f

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

      end
    end

    # Option: Throw an exception if there any required attributes
    # are missing
    #
    raise("'distance' attribute must be specified!") unless @distance
  end
  #####


  #####
  # Override init.
  # Do some initialization steps
  #
  def init(p_seq_number=1)
    self.data_home = p_seq_number

    @state = READY
  end
  #####


  #####
  # Create directory to hold transit images
  #
  def data_home=(p_seq_num)
    mn_zero = p_seq_num > 9 ? "" : "0"
    home_prefix = mn_zero + p_seq_num.to_s
    DataLog.behavior_data_home = @data_home = home_prefix + "-transit_camera"
  end
  #####


  #####
  # This is the callback method for use by ezservo to trigger
  # the prosilica.
  #
  def capture_image
    image_name = DataLog.behavior_data_home + "transit" + @prefix
    Rover.instance.transit_cam.capture_image(image_name)
  end
  #####


  #####
  # Execute a turn to the given heading
  #
  def execute
    if (@state == READY)
      @state = EXECUTING

      ##
      # First, make sure chambers are in the home position
      #
      Rover.instance.stbd_rack.reinit(Rack::Stbd)
      s_rack = Rover.instance.stbd_rack.home_switch(Rack::Stbd)
      Rover.instance.relay.s_rackOff

      Rover.instance.port_rack.reinit(Rack::Port)
      p_rack = Rover.instance.port_rack.home_switch(Rack::Port)
      Rover.instance.relay.p_rackOff

      unless (s_rack && p_rack)
        syslog("! One or more racks are not in home position - move aborted.")
        @state - FINISHED
        return
      end
      
      ##
      # Initialize the motors
      #
      Rover.instance.relay.motorsOff
      Rover.instance.relay.transitOn
      Rover.instance.relay.motorsOn
      r_sleep(5)
      #Rover.instance.new_ez
      ez = Rover.instance.motors

      ##
      # Set-up transit image acquisition
      #
      if (@take_images)
        Rover.instance.relay.transitOn
        ez.set_move_callback(self.method(:capture_image), @image_interval*825)
      end

      ##
      # Move forward
      #
      ez.active_motor_num = "A"
      ez.proportional_gain = 1000
      ez.velocity = 750

# The monitoring thread model is causing problems at the moment
# TODO: Fix it
#
      ez.forward(distance*854)
      syslog("_ finished with forward move")

#####
=begin
      syslog("_ starting move_forward thread")
      move = RoverThread.new do
        Thread.current["name"] = "move_forward forward"

        # Ezservo.forward method blocks until finished
        #
        ez.forward(distance*854)
        syslog("_ finished with forward move")
      end

      # Move has only so much time to finish
      #    
      elapsed_time = 0
      max_time = 70*@distance

      while move.alive?
        if(elapsed_time > max_time)
          syslog("! timing out forward move")
          break
        end
        syslog("_ still moving after #{elapsed_time} of #{max_time}")
        sleep(3)
        elapsed_time += 3
      end
=end
#####

      ##
      # capture one final image at the end of the move.
      #
      Rover.instance.relay.motorsOff
      ez.close

      sleep(20) # to allow camera flash to charge
      self.capture_image() if @take_images

      Rover.instance.relay.transitOff
      
      # Let dust settle after move
      #
      syslog("_ Let dust settle for #{@sleep} minutes")
      r_sleep(@sleep*60)
 
      @state = FINISHED
    else
      syslog("_ Not ready to execute. Run init first, maybe?")
    end
  end
  #####


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

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


  #####
  def MoveForward.test
    require 'rexml/document'
    require 'device/rover'

    Rover.instance

    # Test code here
    # Straight ahead test
    puts("Monitor testlogs/syslog.csv")
    puts("Straight ahead test")
    make_xml = REXML::Document.new(File.new(Misc.find_plan("test_move.xml")))
    tth = MoveForward.new(make_xml.root)
    tth.init
    puts(tth.dump)
    tth.execute

    # Abort test in thread
    puts("Abort test")
    tth.init
    t=tth.run
    sleep(1)
    puts("Aborting now...")
    tth.abort
    t.join

  end
  #####

end
##########

#####
# Standalone unit test (recommended)
#
if __FILE__ == $0
  MoveForward.test
  puts("Done")
end