// Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "touch_noise_filter/single_position_filter.h"

#include <algorithm>
#include <cstddef>
#include <cstdio>

#include <base/basictypes.h>

namespace touch_noise_filter {

namespace {

// Max squared distance between fingers to be considered in the same position.
__s32 kMaxDistance2 = 2 * 2;

// Max squared movement of a finger before it's no longer considered noise.
__s32 kMaxMovement2 = 2 * 2;

// Min duration (s) after which a common position is considered noise.
double kMinDurationSecs = 2;

// Max duration (s) to check for common positions with previous touches.
double kMaxDurationSecs = 4;

// Returns the squared distance between (x1, y1) and (x2, y2).
__s32 Distance2(__s32 x1, __s32 y1, __s32 x2, __s32 y2) {
  return (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
}

}  // namespace {}

SinglePositionFilter::SinglePositionFilter()
    : touches_start_(0), touches_end_(0) {
  for (size_t i = 0; i < TOUCH_NOISE_MAX_SLOTS; i++)
    tracked_slots_[i] = SINGLE_POSITION_MAX_TOUCHES;
}

void SinglePositionFilter::FilterFrame(Frame* previous, Frame* current,
                                       size_t num_slots) {
  // Forget old touches which will no longer be considered for overlap.
  double touch_cutoff = current->timestamp_ - kMaxDurationSecs;
  for (size_t i = touches_start_; i != touches_end_;
       i = (i + 1) % SINGLE_POSITION_MAX_TOUCHES) {
    if (tracked_touches_[i].tracking_id_ == -1)
      continue;
    if (tracked_touches_[i].end_ < touch_cutoff)
      StopTrackingTouch(i);
  }

  for (size_t slot = 0; slot < num_slots; slot++) {
    Finger* cur = &current->fingers_[slot];
    Finger* prev = &previous->fingers_[slot];

    bool arrived = prev->tracking_id_ == -1 && cur->tracking_id_ >= 0;
    bool departed = prev->tracking_id_ >= 0 && cur->tracking_id_ == -1;
    if (departed)
      tracked_slots_[slot] = SINGLE_POSITION_MAX_TOUCHES;
    if (cur->tracking_id_ == -1)
      continue;

    // Track all new touches until they move too far.
    if (arrived)
      TrackTouch(slot, current);

    size_t t_ind = tracked_slots_[slot];
    if (t_ind != SINGLE_POSITION_MAX_TOUCHES) {
      tracked_touches_[t_ind].end_ = current->timestamp_;
      // Stop tracking if touch moves more than sqrt(kMaxMovement2).
      if (Distance2(cur->x_pos_, cur->y_pos_,
                    tracked_touches_[t_ind].x_pos_,
                    tracked_touches_[t_ind].y_pos_) >
              kMaxMovement2) {
        StopTrackingTouch(t_ind);
      } else {
        // Determine duration over which touches have been occuring in this
        // position.
        double duration = 0;
        for (size_t i = touches_start_; i != touches_end_;
             i = (i + 1) % SINGLE_POSITION_MAX_TOUCHES) {
          if (tracked_touches_[i].tracking_id_ == -1)
            continue;
          if (Distance2(cur->x_pos_, cur->y_pos_,
                        tracked_touches_[i].x_pos_,
                        tracked_touches_[i].y_pos_) <=
                  kMaxDistance2)
            duration = std::max(
                duration, current->timestamp_ - tracked_touches_[i].begin_);
        }

        if (duration > kMinDurationSecs) {
          Log("Cancel tracking id %d, in position occurring for %fs",
              cur->tracking_id_, duration);
          cur->canceled_ = true;
        }
      }
    }
  }
}

void SinglePositionFilter::StopTrackingTouch(size_t index) {
  size_t slot = tracked_touches_[index].slot_;
  if (tracked_slots_[slot] == index)
    tracked_slots_[slot] = SINGLE_POSITION_MAX_TOUCHES;
  tracked_touches_[index].tracking_id_ = -1;

  // If first touch is canceled, remove all dead touches.
  if (index == touches_start_) {
    while (tracked_touches_[touches_start_].tracking_id_ == -1 &&
           touches_start_ != touches_end_)
      touches_start_ = (touches_start_ + 1) % SINGLE_POSITION_MAX_TOUCHES;
  }
}

bool SinglePositionFilter::TrackTouch(size_t slot, Frame* frame) {
  size_t index = touches_end_;
  touches_end_ = (touches_end_ + 1) % SINGLE_POSITION_MAX_TOUCHES;
  // If we would reach the start touch index, we cannot track any more touches.
  if (touches_end_ == touches_start_) {
    touches_end_ = index;
    return false;
  }

  tracked_touches_[index].x_pos_ = frame->fingers_[slot].x_pos_;
  tracked_touches_[index].y_pos_ = frame->fingers_[slot].y_pos_;
  tracked_touches_[index].begin_ = frame->timestamp_;
  tracked_touches_[index].end_ = frame->timestamp_;
  tracked_touches_[index].slot_ = slot;
  tracked_touches_[index].tracking_id_ = frame->fingers_[slot].tracking_id_;
  tracked_slots_[slot] = index;
  return true;
}

}  // namespace touch_noise_filter