=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)
    val = val.to_f

    return @avg if (val < 0.0)
    return @avg if (val > 360.0)

    # Record the new value
    #
    old_val = nil
    @values.push(val)
    old_val = @values.shift if (@values.length > @lag)
#    puts("tossing #{old_val} from set") if (old_val)

    # Calculate the average
    #
    old_avg = @avg
    @avg = adj = 0
    count = 0
    @values.each do |v|
      count += 1
      adj = (diff(@avg,v)/count)
      @avg = @avg + adj
    end

    if (@avg < 0.0)
      @avg += 360.0
#      puts("below zero")
    end

    if (@avg > 360.0)
      @avg -= 360.0
#      puts("above 360")
    end

#    puts("old avg = #{old_avg} + #{val} (#{adj})= ")
    return @avg
  end

  def RunAvg.test(r)
    puts(r.avg(359.99))
    puts(r.avg(0.001))
    puts(r.avg(359.999))
    puts(r.avg(0.01))
    puts(r.avg(359.999))
    puts(r.avg(0.1))
    puts(r.avg(359.999))
    puts(r.avg(0.01))
    puts(r.avg(359.999))
    puts(r.avg(1))
    puts(r.avg(358.5))
    return
  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