#--
#******************************************************************************
#* Copyright 1990-2007 MBARI
#* MBARI Proprietary Information. All rights reserved.
#******************************************************************************
#* Summary  : Higher level propulsion motor driver
#* Filename : control_system.rb
#* Author   : A. Chase
#* Project  : Benthic Rover
#* Version  :
#* Created  : Dec-06
#* Modified : Sept-07 RGH  Ensure that motors run slowly during forward move
#******************************************************************************
#++

require 'rover_utils'
require 'rover_thread'

# ControlSystem class provides a higher-level interface to control the
# movement of the rover than does the ezservo motor controller.
#
class ControlSystem
  include LogHelper

  attr_accessor :ticks_per_degree, :allowed_error
  @MAX_TURN_ATTEMPTS = 5 
  @VELOCITY = 500

  # Args: Motor controller interface, compass interface
  #
  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.
  # Causes the Rover to turn to the specified heading.
  #
  def forward(heading, distance)
    syslog("_ in forward method, moving forward #{distance} counts at heading: #{heading}")
    
    # Execute the turn
    turn_to_heading(heading)
    
    # Move forward
    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
      unless (@ez.forward(distance))
        @ez.halt
        sleep(3)
        # Get the lowest encoder position, see how far into the move
        # we got.
        #
        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
      syslog("_ finished with forward move")
    end

    # Move has only so much time to finish
    #    
    elapsed_time = 0
    max_time = 600
    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
  
  # Decides which way to turn by comparing the current heading to the desired
  # heading, starts the move and waits until the current heading is approx.
  # equal to the desired heading, then stops the motors.
  #
  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}")

    # We might already be facing towards the right direction
    if difference.abs < self.allowed_error
      return
    end
    
    # Decide which way to turn (which motor to run)
    #
    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
    
    # Let move have some time to complete
    #
    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
    
    # Success or timeout
    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

  # Calculate the difference in headings taking into accuont the 360/0
  # interface due north.
  #
  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