#--
#******************************************************************************
#* Copyright 1990-2007 MBARI
#* MBARI Proprietary Information. All rights reserved.
#******************************************************************************
#* Summary  : High-level propulsion system controller
#* Filename : navigator.rb
#* Author   : Chase
#* Project  : Benthic Rover
#* Version  : Integrated real and simulated drivers
#* Created  : Spring 07
#* Modified :
#******************************************************************************

require "rover_utils"
require "ezservo"
require "control_system"
require "rover"

class Navigator
  include LogHelper
  
  #this value should come from a config file
  @@counts_per_meter = 854.7
  MAX_CURRENT_WAIT_TIME = 60 #max time in seconds to wait for current 
  
  attr_accessor :shouldWaitForCurrent, :velocity
  
  #========================================================
  def initialize(roverInstance)
    self.shouldWaitForCurrent = false
    self.velocity = 1000
    @rover = roverInstance
    ci = @rover.cm.get("FavorableCurrentDeviation")
    if (ci)
      @current_deviation = ci.to_f
    else
      @current_deviation = 20
    end
    if (ci = @rover.cm.get("MotorCountsPerMeter"))
      @@count_per_meter = ci.to_f
    end
  end
  
  #========================================================
  # Move a certain distance at a certain heading
  #
  def forward(heading, distance)
    syslog("_ forward method, moving at heading #{heading} #{distance} meters")
    
    #first off, turn to the appropriate heading
    ez = @rover.motors
    cs = ControlSystem.new(ez, @rover.acm)
    cs.turn_to_heading(heading)
    sleep 5
    syslog("_ forward method, turn to heading completed.")
    syslog("_ new heading is: #{@rover.acm.average_heading}")
    
    
    #set the desired heading to the same value as the rover's true heading
    #so that wait_for_current will wait for the true downstream current
    heading = @rover.acm.average_heading
    
    #okay, hopefully we're now pointing in the right direction
    if(self.shouldWaitForCurrent)
      #to save power, during this wait_for_current call we should shut down
      #the relays
      wait_for_current(heading)
    end

    # Move
    move_forward(distance)
  end

  def capture_transit_image()
    syslog("_ capturing transit image")
    Rover.instance.transit_cam.capture_image
  end

  #========================================================
  # Move the rover forward
  #  
  def move_forward(distance_in_meters)

    # Command the motor servos to action
    #
    ez = @rover.motors
    ez.active_motor_num = "A"
    counts = (@@counts_per_meter * distance_in_meters).to_i
    syslog("_ moving rover forward #{distance_in_meters}m, #{counts} counts")
    
    # Sync the transit camera to the move and go
    #
    stat = true
    ez.set_move_callback(Rover.instance.transit_cam.method(:capture_image))
    unless ez.forward(counts)
      ez.halt
      sleep(3)
      # Get the lowest encoder position
      #
      ep = ez.encoder_position
      low = counts
      ep.each do |p|
        low = p if p < low
      end
      syslog("! Servo error. Move halted after #{low} counts")

      ez_stat = ez.current_status?
      if (ez_stat && ez_stat == "overload error")
	# Make an adjustment to the motor parameters and try
	# one more time to complete the move.
	#
	syslog("_ Overload error. Try to complete at lower velocity")
	ez.adjust_for_overload      
	# Complete the move
	#
	if (low < counts)
	  stat = ez.forward(counts-low)
	end
 
      elsif (ez_stat && ez_stat == "communications error")
	# If we encounter a rare comms error, just try it again
	#
	syslog("_ Comms error. Try again")
	stat = ez.forward(counts)

      else
	stat = false
      end
    end

    syslog("_ Move complete") if stat
    syslog("_ Move failed") unless stat
    return stat
  end
  
  #========================================================
  # Wait for the current to be going in the opposite direction of the supplied
  # heading. Returns true if appropriate current detected before timeout, false
  # otherwise
  def wait_for_current(heading)
    syslog("_ waiting for current to go opposite this heading: #{heading}")
    desiredCurrent = (heading + 180) % 360

    # Wait for the desired current
    #
    totalTimeWaiting = 0
    while (totalTimeWaiting < Navigator::MAX_CURRENT_WAIT_TIME)

      # What is the difference between the desired and present directions?
      #
      presentCurrent = @rover.acm.avg_current_heading()
      difference = presentCurrent - desiredCurrent

      # Adjust for cases when directions straddle due north
      #
      if(difference.abs > 180)
        difference = difference.abs - 360
      end

      # Now compare and decide
      #
      if(difference.abs < @current_deviation)
	# Close enough
	#
        syslog("_ found a current within +- #{@current_deviation} degrees of #{desiredCurrent}")
        syslog("_ presentCurrent = #{presentCurrent}")
        return true
      else
	# Continue waiting
	#
        Rover.instance.rt.rSleep(5)
        totalTimeWaiting += 5
      end
    end
    syslog("! timed out while waiting for current heading")
    return false
  end
end