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
  #MINI-ROVER
  #@@counts_per_meter = 100
  
  attr_accessor :should_wait_for_current, :velocity
  
  def initialize
    self.should_wait_for_current = false
    self.velocity = 500
  end
  
  #follow heading (0-360) for the specified distance (in meters)
  def forward heading, distance
    Rover.instance.relay.turn_on 1
    Rover.instance.relay.turn_on 2
    
    #first off, start by turning to the appropriate heading
    ez = Ezservo.rover
    #MINI-ROVER
    #ez = Ezservo.mini_rover
    ez.velocity = self.velocity
    ez.log_moves = true
    cs = ControlSystem.new ez, Rover.instance.acm
    cs.turn_to_heading_continuous heading
    
    #okay, hopefully we're now pointing in the right direction
    if(self.should_wait_for_current)
      #to save power, durring this wait_for_current call we should shut down
      #the relays
      wait_for_current(heading)
    end
    
    #don't block on the move, so we can track the heading separately
#    move = Thread.new do
     ez.active_motor_num = "A"
     counts = (@@counts_per_meter * distance).to_i
     syslog "moving rover forward #{distance} meters, #{counts} counts"
     ez.forward counts
#    end
#    elapsed_t = 0
#    max_t = 270 
#    while move.alive? && elapsed_t < max_t
#      syslog "in navigator, calling track_heading"
#      cs.track_heading heading, self.velocity
#      sleep 3
#      elapsed_t += 3
#    end
    
#    Rover.instance.relay.turn_off 1
#    Rover.instance.relay.turn_off 2
     ez.stop_listener
  end
  
  #not yet implemented
  def wait_for_current heading
    syslog "waiting for current to go opposite this heading: #{heading}"
  end
end