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

    move_forward(distance)
    syslog("_ move complete")
  end
  
  def move_forward(distance_in_meters)
    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")
    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. Halting move after #{low} counts")
      syslog("_ Let's try to complete the move at lower velocity")

      ez.adjust_for_overload      
      # Complete the move
      #
      if (low < counts)
        return ez.forward(counts-low)
      end
    end
    false
  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 < @current_deviation)
        syslog("_ found a current within +- #{@current_deviation} 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