#include <iostream>
#include <cmath>
#include <sys/time.h>

#include "fixationfilter.h"

// Nearly verbatim from LC Technologies' fixfunc.c
// Eye Fixation Analysis Functions
// LC Technologies, Inc.
// 9455 Silver King Court
// Fairfax, VA 22031
// (703) 385-7133
// Available on the web, somewhere in the vicinity of 
// http://www.eyegaze.com

// some additions made by ARF: Andrew R. Freed
// original fixfunc.c found at:
// http://www.freedville.com/professional/thesis/eyetrack/source-code/fixfunc.c
// (last accessed 2/7/2009)

// bug fix by ATD: Andrew T. Duchowski

// orig. documentation follows, modified to reflect current C++ implementation

// RETURN VALUES - Eye Motion State:
//
//  MOVING                0   The eye was in motion min_fix_samples ago
//  FIXATING              1   The eye was fixating min_fix_samples ago
//  FIXATION_COMPLETED    2   A completed fixation has just been detected;
//                              the fixation ended min_fix_samples ago
//
// Include fixationfilter.h for class def. and above constant definitions.
//
// SUMMARY
//
//    This function converts a series of uniformly-sampled (raw) gaze
// points into a series of variable-duration saccades and fixations.
// Fixation analysis may be performed in real time or after the fact.  To
// allow eye fixation analysis during real-time eyegaze data collection,
// the function is designed to be called once per sample.  When the eye
// is in motion, ie during saccades, the function returns 0 (MOVING).
// When the eye is still, ie during fixations, the function returns 1
// (FIXATING).  Upon the detected completion of a fixation, the function
// returns 2 (FIXATION_COMPLETED) and produces:
//   a) the time duration of the saccade between the last and present
//      eye fixation (eyegaze samples)
//   b) the time duration of the present, just completed fixation
//      (eyegaze samples)
//   c) the average x and y coordinates of the eye fixation
//      (in user defined units of x_gaze and y_gaze)
// Note: Although this function is intended to work in "real time", there
// is a delay of minimum_fix_samples in the filter which detects the
// motion/fixation condition of the eye.
//
// PRINCIPLE OF OPERATION
//
//    This function detects fixations by looking for sequences of gaze-
// point measurements that remain relatively constant.  If a new gazepoint
// lies within a circular region around the running average of an on-going
// fixation, the fixation is extended to include the new gazepoint.
// (The radius of the acceptance circle is user specified by setting the
// value of the function argument gaze_deviation_threshold.)
//    To accommodate noisy eyegaze measurements, a gazepoint that exceeds
// the deviation threshold is included in an on-going fixation if the
// subsequent gazepoint returns to a position within the threshold.
//    If a gazepoint is not found, during a blink for example, a fixation
// is extended if a) the next legitimate gazepoint measurement falls within
// the acceptance circle, and b) there are less than minimum_fix_samples
// of successive missed gazepoints.  Otherwise, the previous fixation
// is considered to end at the last good gazepoint measurement.
//
// UNITS OF MEASURE
//
//    The gaze position/direction may be expressed in any units (e.g.
// millimeters, pixels, or radians), but the filter threshold must be
// expressed in the same units.
//
// INITIALIZING THE FUNCTION
//
//    Prior to analyzing a sequence of gazepoint data, the initFixation
// function should be called to clear any previous, present and new
// fixations and to initialize the ring buffers of prior gazepoint data.
//
// PROGRAM NOTES
//
// For purposes of describing an ongoing sequence of fixations, fixations
// in this program are referred to as "previous", "present", and "new".
// The present fixation is the one that is going on right now, or, if a
// new fixation has just started, the present fixation is the one that
// just finished.  The previous fixation is the one immediatly preceeding
// the present one, and a new fixation is the one immediately following
// the present one.  Once the present fixation is declared to be completed,
// the present fixation becomes the previous one, the new fixation becomes
// the present one, and there is not yet a new fixation.

FixationFilter::FixationFilter(float gaze_dev_threshold,
                                 int minimum_fix_samples)
{
  gaze_deviation_threshold = gaze_dev_threshold;
  initFixation(minimum_fix_samples);
}

void FixationFilter::initFixation(int minimum_fix_samples)
{
  // this function clears any previous, present and new fixations, and it
  // initializes detectFixation()'s internal ring buffers of prior
  // gazepoint data.  initFixation() should be called prior to a sequence
  // of calls to detectFixation().

  // minimum number of gaze samples that can be considered a fixation
  //   note: if the input value is less than 3, the function sets it to 3
  minimum_fixation_samples = minimum_fix_samples;

  // make sure the minimum fix time is at least 3 samples
  if(minimum_fixation_samples < 3) minimum_fixation_samples = 3;

  if(minimum_fixation_samples >= RING_SIZE) {
    std::cerr << "Warning: minimum_fixation_samples ";
    std::cerr << minimum_fixation_samples;
    std::cerr << " >= RING_SIZE " << RING_SIZE << std::endl;
  }

  // initialize the internal ring buffer
  for(ringIndex = 0; ringIndex < RING_SIZE; ringIndex++) {
    x_gaze_ring[ringIndex] = 0.0F;
    y_gaze_ring[ringIndex] = 0.0F;
    gaze_found_ring[ringIndex] = false;
    eye_motion_state[ringIndex] = MOVING;
    x_fix_ring[ringIndex] = 0.0F;
    y_fix_ring[ringIndex] = 0.0F;
    gaze_deviation_ring[ringIndex] = -0.1F;
    sac_duration_ring[ringIndex] = 0;
    fix_duration_ring[ringIndex] = 0;
  }
  ringIndex = 0;
  ringIndexDelay = RING_SIZE - minimum_fixation_samples;

  // set the call count to 0, init the previous fixation end count
  // so the first saccade duration is a legitimate count
  callCount = 0;
  prevFixEndCount = 0;

  // reset the present fixation data
  resetPresentFixation();

  // reset the new fixation data
  resetNewFixation();

  // initialize the number of successive samples with no eye found
  nNoEyeFound = 0;
}

int FixationFilter::detectFixation(bool gazepoint_found,
                                   float x_gaze, float y_gaze)
{
  // increment the call count, the ring index, and the delayed ring index
  callCount++;
  ringIndex++;
  if(ringIndex >= RING_SIZE) ringIndex = 0;
  ringIndexDelay = ringIndex - minimum_fixation_samples;
  if(ringIndexDelay < 0) ringIndexDelay += RING_SIZE;

  // update the storage rings
  x_gaze_ring[ringIndex]     = x_gaze;
  y_gaze_ring[ringIndex]     = y_gaze;
  gaze_found_ring[ringIndex] = gazepoint_found;

  // initially assume the eye is moving
  // note: these values are updated during the processing of this and
  //       subsequent gazepoints
  eye_motion_state[ringIndex]    = MOVING;
  x_fix_ring[ringIndex]          = -0.0F;
  y_fix_ring[ringIndex]          = -0.0F;
  gaze_deviation_ring[ringIndex] = -0.1F;
  sac_duration_ring[ringIndex]   = 0;
  fix_duration_ring[ringIndex]   = 0;

  // PROCESS TRACKED EYE
  // A1: if the eye's gazepoint was successfully measured this sample
  if(gazepoint_found) {
    nNoEyeFound = 0;		// number of successive no-tracks is zero
    // B1: if there is a present fixation
    if(nPresFixSamples > 0) {
      // compute the deviation of the gaze point from the present fixation
      calculateGazeDeviationFromPresentFixation(x_gaze, y_gaze);
      // C1: if the gaze point is within the present fixation region
      if(presDr <= gaze_deviation_threshold) {
        // restore any previous gazepoints that were temporarily left
        // out of the fixation
        restoreOutPoints();
        // update the present fixation hypothesis and check if there
        // are enough samples to declare that the eye is fixating
        updatePresentFixation(x_gaze, y_gaze);
      } 
      // C2: otherwise (gaze point outside present fixation region)
      else { // presDr > gaze_deviation_threshold
        // increment the number of gazepoint samples outside the present fix.
        nPresOut++;
        // ATD: I think nPresOut has to be (range) limited to RING_SIZE
        if(nPresOut >= RING_SIZE) nPresOut = 0;
        // D1: if the present fixation is finished, i.e., if there have
        //     been minimum_fixation_samples since the gazepoint last matched
        //     the present fixation, and the present fixation is long
        //     enough to count as a real fixation,
        if(((int)(callCount - presFixEndCount) >= minimum_fixation_samples) &&
                 (nPresFixSamples >= minimum_fixation_samples)) {
          // declare the present fixation to be completed, move the
          // present fixation to the prior, move the new fixation to
          // the present, and check if the new (now present) fixation
          // has enough points for the eye to be declared to be fixating
          declareCompletedFixation();
          // compute the deviation of the gazepoint from the now present fix.
          calculateGazeDeviationFromPresentFixation(x_gaze, y_gaze);
          // E1: if the gazepoint is within the now present fixation region
          if(presDr <= gaze_deviation_threshold) {
            // update the present fixation data and check if there
            // are enough samples to declare that the eye is fixating
            updatePresentFixation(x_gaze, y_gaze);
          }
          // E2: otherwise (the gazepoint is outside the present fix. region)
          else {
            // start a new fixation at the gazepoint
            startNewFixationAtGazepoint(x_gaze, y_gaze);
          }
        }
        // D2: otherwise (the present fixation is not finished)
        else  {
          // F1: if there is a new fixation hypothesis
          if(n_new_fix_samples > 0) {
            // compute the deviation of the gazepoint from the new fixation
            calculateGazeDeviationFromNewFixation(x_gaze, y_gaze);
            // G1: if the new point falls within the new fix
            if(new_dr <= gaze_deviation_threshold) {
              // update the new fixation hypothesis
              updateNewFixation(x_gaze, y_gaze);
              // H: if there are now enough points in the new fix
              //    to declare it a real fix
              if(n_new_fix_samples == minimum_fixation_samples) {
                // drop the present fixation data, move the new
                // fixation into the present fixation and see
                // if the new (now present) fixation has enough
                // points to declare the eye to be fixating
                moveNewFixationToPresentFixation();
              }
            } 
            // G2: otherwise (the point is outside the new fixation)
            else {
              // start the new fixation at the new gazepoint
              startNewFixationAtGazepoint(x_gaze, y_gaze);
            }
          }
          // F2: otherwise (there is not a new fixation)
          else {
            // start the new fixation at the gazepoint
            startNewFixationAtGazepoint(x_gaze, y_gaze);
          }
        }
      }
    }
    // B2: otherwise (ther is not a present fixation)
    else {
      // start the present fixation at the gazepoint and reset the new fixation
      startPresentFixationAtGazepoint(x_gaze, y_gaze);
    }
  }
  // PROCESS THE UNTRACKED EYE
  // A2: otherwise (the gazepoint was not successfully measured this sample)
  else {
    // increment the number of successive samples with no eye found
    nNoEyeFound++;
    // I: if it has been minimum_fixation_samples since the last sample
    //    in the present fixation
    if((int)(callCount-presFixEndCount) >= minimum_fixation_samples) {
      // J: if there had been a fixation prior to losing track of the eye
      if(nPresFixSamples >= minimum_fixation_samples) {
        // declare the present fixation to be completed, move the
        // present fixation to the prior, move the new fixation to
        // the present, and check if the new (now present) fixation
        // has enough points for the eye to be declared to be fixating
        declareCompletedFixation();
      }
      // reset the present fixation data
      resetPresentFixation();
    }
  }
  // originally, this code chunk would pass the data back to calling function,
  // passing the delayed gazepoint data, with relevant saccade/fixation data
  x_gaze_delayed = x_gaze_ring[ringIndexDelay];
  y_gaze_delayed = y_gaze_ring[ringIndexDelay];
  gazepoint_found_delayed = gaze_found_ring[ringIndexDelay];
  x_fix_delayed = x_fix_ring[ringIndexDelay];
  y_fix_delayed = y_fix_ring[ringIndexDelay];
  gaze_deviation_delayed = gaze_deviation_ring[ringIndexDelay];
  saccade_duration_delayed = sac_duration_ring[ringIndexDelay];
  fix_duration_delayed = fix_duration_ring[ringIndexDelay];

  // return the eye motion/fixation state for the delayed point
  return(eye_motion_state[ringIndexDelay]);
}

void FixationFilter::resetPresentFixation(void)
{
  // reset the present fixation, i.e., declare it nonexistent
  presFixStartCount   = 0;
  presFixEndCount     = 0;
  nPresFixSamples     = 0;
  xPresFixSum         = 0.0F;
  yPresFixSum         = 0.0F;
  xPresFix            = 0.0F;
  yPresFix            = 0.0F;
  nPresOut            = 0;
}

void FixationFilter::resetNewFixation(void)
{
  // reset new fixation, i.e., declare it nonexistent
  new_fix_start_count = 0;
  new_fix_end_count   = 0;
  n_new_fix_samples   = 0;
  x_new_fix_sum       = 0.0F;
  y_new_fix_sum       = 0.0F;
  x_new_fix           = 0.0F;
  y_new_fix           = 0.0F;
}

void FixationFilter::startPresentFixationAtGazepoint(float x_gaze, float y_gaze)
{
  // starts the present fixation at the argument gazepoint
  // and makes sure there is no new fixation hypothesis
  nPresFixSamples     = 1;
  xPresFixSum         = x_gaze;
  yPresFixSum         = y_gaze;
  xPresFix            = x_gaze;
  yPresFix            = y_gaze;
  presFixStartCount   = callCount;
  presFixEndCount     = callCount;
  nPresOut            = 0;

  // make sure there is no new fixation
  resetNewFixation();
}

void FixationFilter::startNewFixationAtGazepoint(float x_gaze, float y_gaze)
{
  // start new fixation at argument gazepoint
  n_new_fix_samples   = 1;
  x_new_fix_sum       = x_gaze;
  y_new_fix_sum       = y_gaze;
  x_new_fix           = x_gaze;
  y_new_fix           = y_gaze;
  new_fix_start_count = callCount;
  new_fix_end_count   = callCount;
}

void FixationFilter::updatePresentFixation(float x_gaze, float y_gaze)
{
  // update present fixation with the argument gazepoint,
  // checks if there are enough samples to declare that the eye is now
  // fixating, and makes sure there is no hypothesis for a new fixation
  nPresFixSamples++;
  xPresFixSum        += x_gaze;
  yPresFixSum        += y_gaze;
  xPresFix            = xPresFixSum / nPresFixSamples;
  yPresFix            = yPresFixSum / nPresFixSamples;
  presFixEndCount     = callCount;
  nPresOut            = 0;

  // check if there are enough samples in the present fixation hypothesis
  // to declare that the eye is fixating
  checkIfFixating();

  // there is no hypothesis for a new fixation
  resetNewFixation();
}

void FixationFilter::updateNewFixation(float x_gaze, float y_gaze)
{
  // updates the new fixation with the argument gazepoint
  n_new_fix_samples++;
  x_new_fix_sum      += x_gaze;
  y_new_fix_sum      += y_gaze;
  x_new_fix           = x_new_fix_sum / n_new_fix_samples;
  y_new_fix           = y_new_fix_sum / n_new_fix_samples;
  new_fix_end_count   = callCount;
}

void FixationFilter::calculateGazeDeviationFromPresentFixation(float x_gaze, float y_gaze)
{
	float	dx, dy;		// horiz. and vert. deviations

  // calculate the deviation of the gazepoint from the present fixation location
  dx                  = x_gaze - xPresFix;
  dy                  = y_gaze - yPresFix;
  presDr              = (float)sqrt(dx*dx + dy*dy);

  // put the deviation in the ring buffer for future reference
  gaze_deviation_ring[ringIndex] = presDr;
}

void FixationFilter::calculateGazeDeviationFromNewFixation(float x_gaze, float y_gaze)
{
	float	dx, dy;		// horiz. and vert. deviations

  // calculate the deviation of the gazepoint from the new fixation location
  dx                  = x_gaze - x_new_fix;
  dy                  = y_gaze - y_new_fix;
  new_dr              = (float)sqrt(dx*dx + dy*dy);
}

void FixationFilter::checkIfFixating(void)
{
	int	i, ii;		// dummy ring indices

  // checks to see whether there are enough samples in the presently
  // hypothesized fixation to declare that the eye is fixating yet,
  // and if there is a true fixation going on, it updates the ring
  // buffers to reflect the fixation

  // if there are enough samples for a fixation
  if(nPresFixSamples >= minimum_fixation_samples) {
    // declare the eye to be fixating; go back through the last
    // minimum_fixation_samples entries of the ring buffer making sure that all
    // samples from the present fixation are marked as fixating, and set
    // the entries with the newest estimate of the fixation location
    for(i=0; i < minimum_fixation_samples; i++) {

      ii = ringIndex - i;
      if(ii < 0) ii += RING_SIZE;

      eye_motion_state[ii] = FIXATING;
      x_fix_ring[ii] = xPresFix;
      y_fix_ring[ii] = yPresFix;

      sac_duration_ring[ii] = (int)(presFixStartCount - prevFixEndCount-1);
      fix_duration_ring[ii] = (int)(presFixEndCount - presFixStartCount+1-i);
    }
  } 
}

void FixationFilter::moveNewFixationToPresentFixation(void)
{
  // copies the new fixation data into the present fixation
  // and resets the new fixation
  nPresFixSamples     = n_new_fix_samples;
  xPresFixSum         = x_new_fix_sum;
  yPresFixSum         = y_new_fix_sum;
  xPresFix            = x_new_fix;
  yPresFix            = y_new_fix;
  presFixStartCount   = new_fix_start_count;
  presFixEndCount     = new_fix_end_count;
  nPresOut            = 0;

  // reset new fixation
  resetNewFixation();

  // check if there are enough samples in the new (now present) fixation to
  // declare that the eye is fixating
  checkIfFixating();
}

void FixationFilter::declareCompletedFixation(void)
{
  // declare the present fixation to be completed
  eye_motion_state[ringIndexDelay] = FIXATION_COMPLETED;

  // move the present fixation to the previous fixation; this saves the
  // end time of the present fixation for later computation of the saccade
  // period between this and the next fixation
  prevFixEndCount = presFixEndCount;

  // move the new fixation data, if any, to the present fixation, reset
  // the new fixation, and check if there are enough samples in the new
  // (now present) fixation to declare that the eye is fixating
  moveNewFixationToPresentFixation();
}

void FixationFilter::restoreOutPoints(void)
{
	int	i, ii;	// dummy ring indices

  // restores any previous gazepoints that were left out of
  // the fixation and are now known to be part of the present fixation

  // if there were some points that temporarily went out of the fixation region
  if(nPresOut > 0) {

    // undo the hypothesis that they were outside the fixation and declare
    // them now to be part of the fixation
    for(i = 1; i <= nPresOut; i++) {
      ii = ringIndex - i;
      if(ii < 0) ii += RING_SIZE;
      // ATD: I think if nPresOut is not (range) limited to RING_SIZE
      // BUG! ii goes -ve (way -ve, out to -184)
//    std::cerr << "ii = " << ii << std::endl;
      if(gaze_found_ring[ii]) {
        nPresFixSamples++;
        xPresFixSum += x_gaze_ring[ii];
        yPresFixSum += y_gaze_ring[ii];
        eye_motion_state[ii] = FIXATING;
      }
    }
    // set the number of "out" points to be zero
    nPresOut = 0;
  }
}