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

##########
# FavorableCurrent class.
# Controls the behavior when the Rover awaits for the current to flow
# roughly opposite/right-angle to a planned move. Usually occurs before
# a Movement behavior
#
class FavorableCurrent < Behavior
  include RoverTime

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

    @duration = 1    # Maximum duration before giving-up
    @start_time = 0  # Record when behavior started
    @end_time   = 0  # Calculate when it should time-out
    @magnitude = 2   # The current magnitude we're looking for
    @heading = "opp" # Looking for current flowing in this direction...
    @deviation = 45  # ...plus or minus this deviation

    ##
    # attributes used for sleep-sample strategy
    #
    @sample_time     = 0 # Minutes to sample current before sleeping
    @sample_interval = 0 # Minutes between samples

    load_attrs(p_behavior_element)
  end
  #####


  #####
  # A crude documentation feature. Each subclass should provide
  # one that begins by calling this method.
  #
  def FavorableCurrent.usage
    u  = sprintf(Behavior.usage_format, \
                 "duration","Max wait time for current","Minutes","1.0")
    u += sprintf(Behavior.usage_format, \
                 "heading","Optimal current heading","Deg/'opp'","opp")
    u += sprintf(Behavior.usage_format, \
                 "deviation","Allowed deviation from optimal","Degrees","45.0")
    u += sprintf(Behavior.usage_format, \
                 "magnitude","Minimum current velocity","cm/sec","2.0")
    u += sprintf(Behavior.usage_format, \
                 "sample_time","Sampling period before sleeping","Minutes","0.0 (no sleep)")
    u += sprintf(Behavior.usage_format, \
                 "sample_interval","Sleep duration between samples","Minutes","0.0 (no sleep)")
    u += "\n<FavorableCurrent name=\"Example1\" duration=\"60\" "
    u += "sample_time=\"3\" sample_interval=\"7\" heading=\"100\" "
    u += "deviation=\"60\" comment=\"Wait for current of 40-160 degrees; sample for 3 minutes then sleep for 7\" />\n"
    u += "\n<FavorableCurrent name=\"Example2\" duration=\"60\" heading=\"opp\" "
    u += "deviation=\"60\" comment=\"Wait for current heading opposite of vehicle heading +/60; no sleeping\" />\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

        # Total duration of the launch
        when "duration"
        @duration = attrs[k].to_f
        @sample_time = @duration unless (@sample_time > 0)

        # Minutes to sample current before sleeping
        when "sample_time"
        @sample_time = attrs[k].to_f

        # Minutes between samples
        when "sample_interval"
        @sample_interval = attrs[k].to_f

        # Magnitude of current (cm/sec)
        when "magnitude"
        @magnitude = attrs[k].to_f

        # Desired current heading
        when "heading"
        if (attrs[k].downcase == "opp")
          @heading = "opp"  # desired heading opposite vehicle heading
        else
          @heading = attrs[k].to_f
        end

        # Allowed deviation
        when "deviation"
        @deviation = attrs[k].to_f.abs

#       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
    #
    # If we're not hibernating, let's set up for
    # one sample session the length of the duration.
    # Both the sample time and interval must be > 0
    # for multiple sessions.
    #
    unless ((@sample_time > 0) && (@sample_interval > 0) &&
            Supervisor.instance.deployment.hibernate)
      @sample_time = @duration
      @sample_interval = 0
    end

    @start_time = 0
    @end_time = 0
    @state = READY
  end
  #####


  #####
  # Resume after hibernation
  #
  def resume_init()
    syslog( "_ resuming from hibernating #{@sample_interval} minutes")

    ##
    # Get the saved state
    #
    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

    unless((st = bs['start_time']))
      raise "Cannot resume! No start_time data in behavior record"
    end

    unless((et = bs['end_time']))
      raise "Cannot resume! No end_time data in behavior record"
    end

    ##
    # Refer to the existing data directory
    #
    DataLog.set_behavior_data_home = bs["log_dir"]

    @start_time = st
    @end_time = et
    console("_ Ready to resume #{@name}")
    @state = READY
  end
  #####


  #####
  # Save state for resume or restart
  #
  def save_behavior_state()
    # Save my state in the deployment database
    #
    syslog("_ Saving state")

    bs = Hash["type", self.class.to_s,
              "name", @name,
              "log_dir", DataLog.behavior_data_home,
              "start_time", @start_time,
              "end_time", @end_time]

    Supervisor.instance.deployment.dbif.save_behavior_data(bs)
  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

    # Calculate the behavior start and end times on the initial run only,
    # not when resuming.
    #
    @start_time = Time.now.to_i unless (@start_time > 0)
    @end_time   = Time.now.to_i + (@duration * 60.0) unless (@end_time > 0)

    # Figure out how many more seconds for this session
    #
    behavior_remaining_secs = @end_time - Time.now.to_i
    session_remaining_secs  = @sample_time * 60.0

    # Session time vs. behavior time: whichever is least
    # 
    if (behavior_remaining_secs < session_remaining_secs)
      session_remaining_secs = behavior_remaining_secs
    end
    syslog("_ #{behavior_remaining_secs} seconds remaining in duration")
    syslog("_ #{session_remaining_secs} seconds in this session")

    # Perform the behavior if behavior has not timed-out
    #
    if (behavior_remaining_secs > 0)
      begin
        #
        # Let's watch if this is a mars deployment
        #
        if (Supervisor.instance.deployment.mars)
          Rover.instance.relay.camerasOn
          Rover.instance.relay.videoOn
        end

        ##
        # Begin measuring the current direction
        #
        # TODO: Ensure that the acm is reporting non-zero
        #
        Rover.instance.relay.acmOn()
        sleep(2)
        Rover.instance.acm.run()
        sleep(5)


        ##
        # A useful "special" case:
        # Look for current direction opposite the vehicle's heading?
        #
        if (@heading == "opp")
          @heading = Rover.instance.acm.vehicle_heading - 180.0
          @heading += 360 if (@heading < 0)
        end

        # Prepare a blurb for logging
        #
        need = "Need #{@magnitude}@#{@heading}+/-#{@deviation}."

        ##
        # Check the current direction until we detect a favorable current
        # or our alloted time has expired.
        #
        while (session_remaining_secs > 0)

          ## wait a spell, gather data, check the readings ##
          r_sleep(10)
          session_remaining_secs -= 10
          behavior_remaining_secs -= 10

          ## get the heading and magnitude of the current ##
          ch = Rover.instance.acm.current_heading
          mag = Rover.instance.acm.current_magnitude

          ## do the math. stop looking if we're out of time or good current ##
          diff = (@heading - ch).abs
          diff = 360 - diff if (diff > 180)
          syslog("_ difference is #{diff}")
          if ((diff.abs <= @deviation) &&
              (mag >= @magnitude))
            wt = (Time.now.to_i - @start_time)/60.0
            syslog("_ Favorable current detected after #{wt} minutes")
            syslog("_ Dir: #{ch}")
            syslog("_ Mag: #{mag}")
            break
          end

          ## write a blurb and keep looking ##
          syslog("_ mag/dir: #{mag.round}@#{ch.round}. #{need}")
        end

        ##
        # Session timed-out.
        # Hibernate unless the behavior duration has been exceeded
        #
        if (session_remaining_secs <= 0)
          syslog("_ Sample session timed-out")

          if (behavior_remaining_secs <= 0)
            syslog("_ Behavior timed-out while sampling")
          else
            #
            # Prepare for hibernation and throw exception
            #
            if (Supervisor.instance.deployment.hibernate)
              syslog("_ Hibernate #{@sample_interval} minutes")
              save_behavior_state()

              raise Hibernate.new(@sample_interval)

            end
          end
        end

      ensure
        self.clean_up()
      end
    else
      syslog("_ Behavior timed-out while hibernating")
    end

    self.clean_up()
  end
  #####


  #####
  # Clean-up steps in the case of an exception or an abort
  #
  def clean_up
    syslog("_ clean up")
    Rover.instance.relay.acmOff
    Rover.instance.acm.stop
    @state = FINISHED
  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 function. Use to run standard test of FavorableCurrent from
  # a ruby script.
  # 
  def FavorableCurrent.test
    require 'rexml/document'
    require 'device/rover'
    require 'factory'

    Rover.instance

    # Test code here
    # Straight ahead test
    puts("Straight ahead test")
    fc_xml = REXML::Document.new(File.new(Misc.find_plan("test_favorable_current.xml")))
    fc = FavorableCurrent.new(fc_xml.root)
    fc.init
    puts(fc.dump)
    fc.execute

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

    # Add negative duration value to trigger timeout
    puts("duration test")
    fc_xml.root.add_attribute("duration","-1.0")
    fc = Factory.behavior(fc_xml.root)
    fc.init
    puts(fc.dump)
    fc.execute

    # Add large magnitude to force a timeout
    puts("magnitude test")
    fc_xml = REXML::Document.new(File.new(Misc.find_plan("test_favorable_current.xml")))
    fc_xml.root.add_attribute("magnitude","15")
    fc = Factory.behavior(fc_xml.root)
    fc.init
    t=fc.run
    t.join

    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(FavorableCurrent.usage)
    exit(0)
  end

  FavorableCurrent.test
end