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

require 'rover_utils'

class ControlSystem
  include LogHelper

  attr_accessor :ticks_per_degree, :allowed_error

  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
  
  def turn_to_heading desired_heading
    syslog "turning to #{desired_heading}"
    heading = @compass.average_heading
    syslog "current heading is #{heading}"
    difference = calculate_difference desired_heading
    tries = 0
    while difference.abs > self.allowed_error && tries < 5 
      distance = (difference.abs * self.ticks_per_degree).round
      #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 #{distance} starboard #{starboard_dir}, port #{port_dir}"
      @ez.turn starboard_dir, distance, port_dir, distance
      heading = @compass.average_heading
      syslog "heading after turn = #{heading}"
      difference = calculate_difference desired_heading
      tries += 1
    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
    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 = Thread.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
    if t.alive?
      t.kill
    end
    sleep 5
    syslog "finished with turn_to_heading_continuous." <<
           " New heading is: #{@compass.average_heading}"
  end      
  
  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
    
  #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
  
  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
