###################### 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
    self.allowed_error = 5
    @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_continuous 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]
    puts "encoder positions = #{@ez.encoder_position}"
    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
      unless self.on_course?(heading)
        syslog "rover has drifted off course, stopping and correcting"
        @ez.halt
        sleep 1
        @ez.active_motor_num = "A"
        current_encoder_count = @ez.encoder_position[0]
        puts "encoder positions = #{@ez.encoder_position}"
        puts "current count = #{current_encoder_count}"
        puts "starting count = #{starting_encoder_count}"
        puts "distance = #{distance}"
        future_move = distance - (current_encoder_count - starting_encoder_count)
        puts "future_move = #{future_move}"
        turn_to_heading_continuous heading
        syslog "starting a new forward move, " + 
               "heading: #{heading}, distance: #{future_move}"
        return forward(heading, future_move)
      end
      sleep 3
      elapsed_time += 3
    end
  end
  
  def turn_to_heading_continuous 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
    #if the difference is positive, move the port side tracks forward and
    #the starboard tracks backwards
    if difference > 0
      port_dir = :forward
      starboard_dir = :backward
    else
      port_dir = :backward
      starboard_dir = :forward
    end
    syslog "moving starboard #{starboard_dir}, port #{port_dir}"
    #don't actually turn continuously (which would be a distance of 0), instead
    #just give a long move to save guard against an unterminate move
    #put this move in it's own thread so it doesn't block and we can observe
    #the compass to stop when heading is right
    t = RoverThread.new do
      @ez.turn starboard_dir, 2000, port_dir, 2000 
    end
    max_t = 180
    elapsed_t = 0
    while difference.abs > self.allowed_error && elapsed_t < max_t
      difference = calculate_difference(desired_heading)
      syslog "heading: #{@compass.average_heading}"
      sleep(.5)
      elapsed_t += .5
    end
    @ez.stop_command
    sleep 2
    if t.alive?
      t.kill
    end
    sleep 3
    syslog "finished with turn_to_heading_continuous." <<
           " New heading is: #{@compass.average_heading}"
  end      
  
  def on_course? desired_heading
    difference = calculate_difference(desired_heading)
    return !(difference.abs > self.allowed_error)
  end
    
  #NOTE 20061206 ACC - This method does not work. You cannot change the velocity
  #through the servo while the servo is making a forward move in position mode. For
  #this to work the servo would need to be in velocity mode.
  #if heading is off by more than the allowed error, velocity will be reduced by
  #10% on the tread inside tread, where "inside" is defined by the turn needed to
  #get back on heading 
  def track_heading desired_heading, base_velocity
    syslog "tracking heading #{desired_heading}"
    difference = calculate_difference(desired_heading) 
    if(difference.abs > self.allowed_error)
      increased_v = (base_velocity * 1.2).round
      slowed_v = (base_velocity * .8).round
      if(difference > 0 && @tracking_change <= 0)
        motor_num = 1
        @ez.active_motor_num = motor_num
        @ez.velocity = slowed_v
        sleep(1)
        motor_num = 2
        @ez.active_motor_num = motor_num
        @ez.velocity = increased_v 
        @tracking_change = 1
      elsif(@tracking_change >= 0)
        motor_num = 2
        @ez.active_motor_num = motor_num
        @ez.velocity = slowed_v
        sleep(1)
        motor_num = 1
        @ez.active_motor_num = motor_num
        @ez.velocity = increased_v 
        @tracking_change = -1
      end
      sleep (.5)
      @ez.active_motor_num = "A"
    elsif(@tracking_change != 0)
      syslog "changing velocities back to base value of #{base_velocity}"
      @ez.active_motor_num = "A"
      @ez.velocity = base_velocity
      @tracking_change = 0
    end
  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
  
  def calibrate_ticks_per_degree 
    syslog "calibrating..."
    heading = @compass.average_heading
    syslog "current heading = #{heading}"
    syslog "turning 500 counts to starboard"
    @ez.turn :backward, 500, :forward, 500
    sleep 2
    new_heading = @compass.average_heading
    syslog "new heading = #{new_heading}"
    delta = (heading - new_heading).abs
    self.ticks_per_degree = 500 / delta
    syslog "new ticks per degree value = #{self.ticks_per_degree}"
    #sanity check
    if(self.ticks_per_degree < 1)
      syslog "ticks per degree is too low, setting value to 1"
      self.ticks_per_degree = 1
    end
  end
end