###################### control_system.rb achase@mbari.org ####################
#
# Separate out the control system from the motor controller
#
###############################################################################

require 'rover_utils'
require 'rover_thread'

class ControlSystem
  include LogHelper

  attr_accessor :ticks_per_degree, :allowed_error
  MAX_TURN_ATTEMPTS = 5 
  VELOCITY = 1000

  def initialize ezservo, compass
    @ez = ezservo
    @compass = compass
    #a rough estimate of encoder_ticks/degree (both tracks in opposite directions)
    self.ticks_per_degree = 10
    #number of degrees of error allowed in seeking a heading, plus or minus
    self.allowed_error = 4
    @tracking_change = 0
  end
  
  #follow the heading (0-360) for the specified number of counts
  def forward heading, distance
    syslog("_ in forward method, moving forward #{distance} counts at heading: #{heading}")
    turn_to_heading heading
    
    syslog("_ setting velocity to #{VELOCITY}")
    syslog("_ setting motor number to A")
    @ez.active_motor_num = "A"
    @ez.velocity = VELOCITY
    syslog("_ starting move thread")
    starting_encoder_count = @ez.encoder_position[0]
    move = RoverThread.new do
      syslog("_ inside move thread")
      @ez.forward distance
      syslog("_ finished with forward move")
    end
    
    elapsed_time = 0
    max_time = 470
    while move.alive?
      #NOTE: This should be a configured value and probably should be calculated
      #based on the size of the forward move
      if(elapsed_time > max_time)
        syslog("! timing out forward move")
        break
      end
      sleep 3
      elapsed_time += 3
    end
  end
  
  def turn_to_heading desired_heading
    syslog("_ using continuous motion to move to heading : #{desired_heading}")
    heading = @compass.average_heading
    syslog("_ current heading is #{heading}")
    difference = calculate_difference desired_heading
    syslog("_ difference between current and desired heading: #{difference}")
    if difference.abs < self.allowed_error
      return
    end
    port_dist = 3000
    starboard_dist = 3000
    if difference > 0
      port_dir = :forward
      starboard_dir = :forward
      starboard_dist = 5
    else
      port_dir = :forward
      starboard_dir = :forward
      port_dist = 5
    end
    syslog("_ moving starboard #{starboard_dir}, port #{port_dir}")
    #make a long move and stop the motors when the heading is where we want it
    #put this move in it's own thread so it doesn't block and we can observe
    #the compass to stop when heading is correct
    t = RoverThread.new do
      @ez.turn starboard_dir, starboard_dist, port_dir, port_dist
    end
    max_t = 180
    elapsed_t = 0
    while difference.abs > self.allowed_error && elapsed_t < max_t
      difference = calculate_difference(desired_heading)
      if(elapsed_t % 5 == 0)
        syslog("_ heading: #{@compass.average_heading}")
      end
      sleep(1)
      elapsed_t += 1
    end
    if(elapsed_t >= max_t)
      syslog("! turn to heading timed out.")
    end
    @ez.stop_command
    sleep 2
    if t.alive?
      t.kill
    end
    sleep 3
    syslog("_ finished with turn_to_heading." <<
           " New heading is: #{@compass.average_heading}")
  end      
  
#~~~~~~~~~~~~~~~~~~~~~PRIVATE METHODS~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
private

  
  def calculate_difference desired_heading
    heading = @compass.average_heading
    difference = desired_heading - heading
    if difference > 180
      difference = desired_heading - 360 - heading
    elsif difference < -180
      difference = desired_heading + 360 - heading
    end
    difference
  end
end