=begin
 ******************************************************************************
 * Copyright 1990-2009 MBARI
 * MBARI Proprietary Information. All rights reserved.
 ******************************************************************************
 * Summary  : Class that can maintian running average of heading values
 * Filename : run_avg.rb
 * Author   : Henthorn
 * Project  : Benthic Rover
 * Version  : 1
 * Created  : Nov 08
 * Modified : 
 ******************************************************************************
=end

# Refer to Rover-code home directory as base
#
require "#{ENV['ROVER_HOME']}/utils/rover_environment"
#

##########
# RunAvg class maintains a running average of heading values ranging from
# 0.0 to 359.99999 degrees.
#
class RunAvg
  attr_reader :lag, :values

  ##
  # Define the filter lag and initial value
  def initialize(lag, initial=nil)
    @lag = lag
    @values = Array.new
    if (initial)
      @avg = initial.to_f
      @values.push(@avg)
    else
      @avg = 0.0
    end
  end

  ##
  # Clear the baffles
  #
  def clear
    @values = Array.new
  end

  ##
  # Helper function returns the relative difference between
  # to heading values.
  #
  def diff(reference, test)
    d = test.to_f - reference.to_f

    # Fix the value if the headings cross the 0/360 degree threshold
    #
    if (d > 180)
      d = -(360 - d)
    elsif (d < -180)
      d = 360 + d
    end
    d
  end

  ##
  # Calculate and return the running average.
  #
  def avg(val=nil)
    return @avg unless (val)

    # Record the new value
    #
    @values.push(val.to_f)
    @values.shift if (@values.length > @lag)

    # Calculate the average
    #
    @avg = 0
    count = 0
    @values.each do |v|
      count += 1
      @avg = @avg + (diff(@avg,v)/count)
      @avg = 360.0 + @avg if (@avg < 0.0)
    end
    return @avg
  end

  def RunAvg.test(r)
    puts(r.avg(10))
    puts(r.avg(5))
    puts(r.avg(3))
    puts(r.avg(2))
    puts(r.avg(5))
    puts(r.avg(359))
    puts(r.avg(357))
    puts(r.avg(355))
    puts(r.avg(353))
    puts(r.avg(351))
    puts(r.avg(350))
    puts(r.avg(349))
  end
end

##
# Standalone unit test (recommended)
# Include at the end of the file as a hook to run a unit test of
# the code from the command line (e.g., "$ ruby  my_class.rb")
#
if __FILE__ == $0
  # Test code here
  RunAvg.test(RunAvg.new(10))
end