=begin
 ******************************************************************************
 * Copyright 1990-2009 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Implementation of the Rover MakeHeading class
 * Filename : make_headin.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'

##########
# MakeHeading behavior - turn vehicle to desired heading
#
class MakeHeading < Behavior
  include SyslogWriter

  ##
  # Attributes
  #
  attr_reader :name, :desired_heading

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

    @exception = false         # Throw an exception during execute
    @desired_heading = nil     # Must be specified in the MakeHeading element
    @allowed_error = 4         # Plus or minus 4 degrees
    @name = self.class.name
    load_attrs(p_behavior_element)

  end
  #####


  #####
  # A crude documentation feature. Each subclass should provide
  # one that begins by calling this method.
  #
  def MakeHeading.usage
    u  = sprintf(Behavior.usage_format, \
                 "desired_heading","Desired vehicle heading","Degrees","None (required)")
    u += "\n<MakeHeading name=\"Example\" desired_heading=\"98.6\" "
    u += "comment=\"Turn vehicle to heading of 98.6 degrees\" />\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 "desired_heading"
      @desired_heading = attrs[k].to_f

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

      end
    end

    # Option: Throw an exception if there any required attributes
    # are missing
    #
    raise("'desired_heading' attribute must be specified!") unless @desired_heading
    raise("Invalid value for 'desired_heading': #{@desired_heading} ") unless (@desired_heading >= 0.0)
    raise("Invalid value for 'desired_heading': #{@desired_heading} ") unless (@desired_heading <= 360.0)
  end
  #####


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

    @state = READY
  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 (@state == READY)
      @state = EXECUTING
      rover = Rover.instance
      current_heading = 0.0

      ##
      # Don't bother running the behavior unless the ACM health
      # is OK.
      #
      acm_state = Supervisor.instance.dbif.load_component_state(RoverStore::Acm)
      unless(acm_state[RoverStore::Health])

        ##
        # Turn on acm compass and start the driver, acquire some data
        # We may need to give it a few attempts.
        #
        for i in 1..4
          rover.relay.acmOn
          sleep(2)

          # Start acquisition thread and give it some time
          #
          rover.acm.run
          sleep(10)

          # A heading of exactly zero means the acm is not srteaming data
          #
          current_heading = rover.acm.vehicle_heading
          if (current_heading == 0.0)
            syslog("_ bad compass readings, try again...")

            # Stop acquisition, power-cycle and repeat
            #
            rover.acm.stop
            rover.relay.acmOff
            sleep(1)
          else
            break
          end
        end
      else
        syslog("_ bad compass checkup, skipping...")
      end

      # Not able to determine the vehicle heading
      #
      if (current_heading == 0.0)
        @state = FINISHED
        return
      end
      
      # Calculate turn parameters
      #
      diff = calculate_heading_difference(@desired_heading, current_heading)
      syslog("_ current vehicle heading is #{current_heading}")
      syslog("_ difference between current and desired heading: #{diff}")

      # Decide which way to turn (which motor to run)
      # Turn right if difference is positive, otherwise left
      # and calculate the number of counts for the motor
      if (diff > 0)
        starboard_dist = 2
        port_dist = (17*diff).abs.round + 50
      else
        starboard_dist = (17*diff).abs.round + 50
        port_dist = 2
      end
      syslog("_ calculated motor counts: star(#{starboard_dist}) port(#{port_dist})")

      ##
      # 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
      
      rover.relay.motorsOn
      sleep(2)

      ez = rover.motors
      ez.velocity = 750

      port_dir = :forward
      starboard_dir = :forward
      t = RoverThread.new do
        Thread.current["name"] = "MakeHeading turn"
        ez.turn(starboard_dir, starboard_dist, port_dir, port_dist)
      end

      # Let move have some time to complete
      #
      max_t = 180
      elapsed_t = 0
      while (diff.abs > @allowed_error && elapsed_t < max_t && t.alive?)
        current_heading = rover.acm.vehicle_heading
        diff = calculate_heading_difference(@desired_heading, current_heading)
        if(elapsed_t % 5 == 0)
          syslog("_ heading: #{current_heading}")
        end
        sleep(0.25)
        elapsed_t += 0.25
      end

      ez.stop_command
      sleep(1)

      # Success or timeout
      if(elapsed_t >= max_t)
        syslog("! turn to heading timed out.")
      end

      if t.alive?
        t.kill
        sleep(1)
      end
      syslog("_ New heading is: #{rover.acm.vehicle_heading}")

      ez.close
      rover.relay.motorsOff
      rover.acm.stop
      rover.relay.acmOff

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


  ######
  # Calculate the difference in headings taking into account the 360/0
  # interface due north.
  #
  def calculate_heading_difference(desired_heading, current_heading)
    difference = desired_heading - current_heading
    if (difference > 180)
      difference -= 360
    elsif (difference < -180)
      difference += 360
    end
    difference
  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 MakeHeading.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_make.xml")))
    tth = MakeHeading.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(MakeHeading.usage)
    exit(0)
  end

  MakeHeading.test
  puts("Done")
end