/*****************************
File:       VecFilter.CPP
Language:   C++ (header)
Project:    H3DNetworkingUtils
The contents of this file are subject to the Mozilla Public License
Version 1.1 (the "License"); you may not use this file except in
compliance with the License. You may obtain a copy of the License at
http://www.mozilla.org/MPL/

Software distributed under the License is distributed on an "AS IS"
basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
License for the specific language governing rights and limitations
under the License.

The Original Code is H3DNetworkingUtils v1.0.

The Initial Developer of the Original Code is CSIRO.
Portions created by the Initial Developer are Copyright (C) 1009 CSIRO. All Rights Reserved.
Contributor(s):
    Chris Gunn  <Chris.Gunn@csiro.au> <ChrisJGunn@gmail.com>
***************************/
#include "H3DNetworkingUtils/VecFilter.h"

using namespace H3DUtil;
using namespace H3DUtil::ArithmeticTypes;
using namespace H3DNetworkingUtils;

// ---------------------------------------------------------------------
VecFilter::VecFilter(int sample_sz) :
sample_size (sample_sz),
filling_samples (true),
num_samples (0) {}

// ---------------------------------------------------------------------
Vec3f VecFilter::filter(Vec3f const & raw_val, double current_time) {
  // This is a weighted averaging filter.
  // The weighting applied to a sample point is sin(t) / t , where t is the
  // time since the sample occurred.
  // It returns one filterd value.

  if (filling_samples) {
    // Add to the samples being averaged.
    sample_vals.push_back(raw_val);
    sample_times.push_back(current_time);
    num_samples++;
    if (num_samples == sample_size)
      filling_samples = false;
  }
  else {
    // Replace an old sample from those being averaged.
    sample_vals.pop_front();
    sample_times.pop_front();
    sample_vals.push_back(raw_val);
    sample_times.push_back(current_time);
  }

  Vec3f filtered_val(0.0, 0.0, 0.0);
  int i;
  for (i = 0; i < num_samples-1; i++) {
    double mult = sin(current_time - sample_times[i]) /
    (current_time  - sample_times[i]);
    filtered_val += sample_vals[i] * mult;
  }
  // We can't go right up to the current time because that would introduce a
  // div by zero. For the current time we add a fudge factor.
  filtered_val += sample_vals[i] * 0.5;
  filtered_val /= num_samples;
  return filtered_val;
}

// ---------------------------------------------------------------------
void VecFilter::resetValue(int sample_sz) {
  num_samples = 0;
  sample_size = sample_sz;
  sample_vals.clear();
  sample_times.clear();
  filling_samples = true;
}
