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 = true
    self.velocity = 1000
    @rover = roverInstance
  end
  
  #follow heading (0-360) for the specified distance (in meters)
  def forward heading, distance
    syslog("_ forward method, moving at heading #{heading} #{distance} meters")
    
    #first off, start by turning 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, durring this wait_for_current call we should shut down
      #the relays
      wait_for_current(heading)
    end
    
    ez.active_motor_num = "A"
    counts = (@@counts_per_meter * distance).to_i
    syslog("_ moving rover forward #{distance} meters, #{counts} counts")
    ez.forward counts
    
    syslog("_ move complete")
  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
    totalTimeWaiting = 0
    while (totalTimeWaiting < Navigator::MAX_CURRENT_WAIT_TIME)
      presentCurrent = @rover.acm.avg_current_heading()
      difference = presentCurrent - desiredCurrent
      if(difference.abs > 180)
        difference += 360
      end
      if(difference.abs < 20)
        syslog("_ found a current within +- 20 degrees of #{desiredCurrent}")
        syslog("_ presentCurrent = #{presentCurrent}")
        return true
      else
        sleep 5
        totalTimeWaiting += 5
      end
    end
    syslog("! timed out while waiting for current heading")
    return false
  end
end