=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 = ""
    @resume = false
    @sleep = 0                 # Minutes to sleep after the move
    load_attrs(p_behavior_element)

  end
  #####


  #####
  # A crude documentation feature. Each subclass should provide
  # one that begins by calling this method.
  #
  def MoveForward.usage
    u  = sprintf(Behavior.usage_format, \
                 "distance","Distance to move","Meters","None (required)")
    u += sprintf(Behavior.usage_format, \
                 "take_images","Take images during transit?","true/false","true")
    u += sprintf(Behavior.usage_format, \
                 "image_interval","Sets interval between images","Meters","1.0")
    u += sprintf(Behavior.usage_format, \
                 "prefix","Prepends to image filenames","String","blank")
    u += sprintf(Behavior.usage_format, \
                 "sleep","How long to hibernate after move","Minutes","0")
    u += "\n<MoveForward name=\"Example\" distance=\"5.5\" sleep=\"60\" "
    u += "image_interval=\"1.25\" prefix=\"transit\" "
    u += "comment=\"Move 5 meters;image every 1.25m;sleep 1 hour\" />\n"
  end


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

    attrs = p_behavior_element.attributes
    attrs.keys.each do |k|
      case k

      when "name"
        # Handled by super

      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
  #####


  #####
  # Resume from hibernation
  #
  def resume_init()
    syslog( "_ resuming from hibernating #{@sleep} minutes")
    bs = Supervisor.instance.deployment.dbif.load_behavior_data
    type = bs['type']
    unless (type == self.class.to_s)
      raise "Cannot resume! Mismatched Behavior types: #{type}!=#{self.class}"
    end

    name = bs['name']
    unless (name == @name)
      raise "Cannot resume! Mismatched Behavior names: #{name}!=#{@name}"
    end

    DataLog.set_behavior_data_home = bs["log_dir"]
    @state = READY
    @resume = true
  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
  #####

  #####
  # Return 0 if both racks are in the home position.
  # Return Stbd::Rack if only the Stbd rack is not in the home position.
  # Return Port::Rack if only the Port rack is not in the home position.
  # Return Stbd::Rack+Port::Rack if both racks are not in the home position.
  #
  def racks_home
    Rover.instance.relay.s_rackOn
    Rover.instance.stbd_rack.reinit
    s_rack = Rover.instance.stbd_rack.home_switch?
 
=begin
    ##
    # If not at home, examine the health of the rack.
    # If the rack controller health is bad, then assume it's OK to move.
    #
    unless (s_rack)
      state = Supervisor.instance.dbif.load_component_state(RoverStore::SRack)
      h = state[RoverStore::Health] if (state)
      if (h && (h != RoverStore::InsertSwitch))
        syslog("_ rack health is bad, OK to move")
        s_rack = true
      end
    end
=end

    Rover.instance.relay.s_rackOff

    Rover.instance.relay.p_rackOn
    Rover.instance.port_rack.reinit
    p_rack = Rover.instance.port_rack.home_switch?
 
=begin
    ##
    # If not at home, examine the health of the rack.
    # If the rack controller health is bad, then assume it's OK to move.
    #
    unless (p_rack)
      state = Supervisor.instance.dbif.load_component_state(RoverStore::PRack)
      h = state[RoverStore::Health] if (state)
      if (h && (h != RoverStore::InsertSwitch))
        syslog("_ rack health is bad, OK to move")
        p_rack = true
      end
    end
=end

    Rover.instance.relay.s_rackOff
    Rover.instance.relay.p_rackOff

    val = 0
    val += Rack::Stbd.to_i unless (s_rack)
    val += Rack::Port.to_i unless (p_rack)
    return val
  end

  #####
  # Attempt to move the selected rack home
  #
  def home_rack(rack_id)
    if (rack_id == Rack::Stbd)
      Rover.instance.relay.s_rackOn
      Rover.instance.stbd_rack.reinit
      Rover.instance.stbd_rack.move_home
      Rover.instance.relay.s_rackOff
    end
    if (rack_id == Rack::Port)
      Rover.instance.relay.p_rackOn
      Rover.instance.port_rack.reinit
      Rover.instance.port_rack.move_home
      Rover.instance.relay.p_rackOff
    end
  end

  #####
  # Execute a turn to the given heading
  #
  def execute

    ##
    # If we're resuming a deployment, we must be done with the
    # the dust settle phase. So, we're done.
    #
    if (@resume)
      @state = FINISHED
      console("_ Resuming MoveForward behavior")
      syslog("_ Back from dust-settle hibernation. Done")
      @resume = false
      return
    end

    ##
    # Don't bother running the behavior unless the health
    # of the prop motors is OK.
    #
    health = Supervisor.instance.dbif.load_component_state(RoverStore::Motors)
    if (health[RoverStore::Health])
      @state = FINISHED
      syslog("! Propulsion motors health is poor. No go")
      return
    end

    ##
    # Move forward after checking on racks
    #
    if (@state == READY)
      @state = EXECUTING

      ##
      # First, make sure chambers are in the home position
      #
      val = racks_home()
      syslog("_ racks_home() == #{val}")
      unless (val == 0)
        # Try to move the racks home
        #
        syslog("! One or more racks are not in home position.")
        home_rack(Rack::Stbd) if (val == Rack::Stbd.to_i || val > 2)
        home_rack(Rack::Port) if (val == Rack::Port.to_i || val > 2)
        # One more check
        #
        unless (racks_home())
          syslog("! One or more racks cannot home - move aborted.")
          @state = FINISHED
          return
        end
      end
      
      ##
      # Initialize the motors
      #
      Rover.instance.relay.motorsOff
      Rover.instance.relay.camerasOff
      Rover.instance.relay.videoOff
      Rover.instance.relay.motorsOn
      r_sleep(3)

      ez = Rover.instance.motors

      ##
      # Set-up transit image acquisition
      #
      if (@take_images)
        Rover.instance.ethernet(true)
        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) if (distance > 0)
      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

      syslog("_ capture one final image at the end of the move")
      r_sleep(30) # to allow camera flash to charge
      self.capture_image() if @take_images

      Rover.instance.relay.transitOff
      Rover.instance.ethernet(false)
      
      # Let dust settle after move
      #
      if (Supervisor.instance.deployment.hibernate && @sleep > 2)
        syslog( "_ initiating hibernation of #{@sleep} minutes")

        ##
        # Save my state in the deployment database
        #
        save_behavior_state()
        raise Hibernate.new(@sleep)

      else
        syslog("_ Let dust settle for #{@sleep} minutes")
        r_sleep(@sleep*60)
      end

      @state = FINISHED
    else
      syslog("_ Not ready to execute. Run init first, maybe?")
    end
  end
  #####


  #####
  def save_behavior_state()
    bs = Hash["type", self.class.to_s,
              "name", @name,
              "log_dir", DataLog.behavior_data_home]

    Supervisor.instance.deployment.dbif.save_behavior_data(bs)
  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
  if "usage" == ARGV[0]
    puts(Behavior.usage)
    puts(MoveForward.usage)
    exit(0)
  end

  MoveForward.test
  puts("Done")
end