// Copyright 2012 The ChromiumOS Authors
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "include/iir_filter_interpreter.h"

#include <utility>
#include <vector>

namespace gestures {

void IirFilterInterpreter::IoHistory::Increment() {
  out_head = NextOutHead();
  in_head = NextInHead();
}

// Uses exact comparisions, rather than approximate float comparisions,
// for operator==
bool IirFilterInterpreter::IoHistory::operator==(
    const IirFilterInterpreter::IoHistory& that) const {
  for (size_t i = 0; i < kInSize; i++)
    if (in[i] != that.in[i])
      return false;
  for (size_t i = 0; i < kOutSize; i++)
    if (out[i] != that.out[i])
      return false;
  return true;
}

// The default filter is a low-pass 2nd order Butterworth IIR filter with a
// normalized cutoff frequency of 0.2.
IirFilterInterpreter::IirFilterInterpreter(PropRegistry* prop_reg,
                                           Interpreter* next,
                                           Tracer* tracer)
    : FilterInterpreter(nullptr, next, tracer, false),
      using_iir_(true),
      b0_(prop_reg, "IIR b0", 0.0674552738890719),
      b1_(prop_reg, "IIR b1", 0.134910547778144),
      b2_(prop_reg, "IIR b2", 0.0674552738890719),
      b3_(prop_reg, "IIR b3", 0.0),
      a1_(prop_reg, "IIR a1", -1.1429805025399),
      a2_(prop_reg, "IIR a2", 0.412801598096189),
      iir_dist_thresh_(prop_reg, "IIR Distance Threshold", 10),
      adjust_iir_on_warp_(prop_reg, "Adjust IIR History On Warp", false) {
  InitName();
  b0_.SetDelegate(this);
  b1_.SetDelegate(this);
  b2_.SetDelegate(this);
  b3_.SetDelegate(this);
  a1_.SetDelegate(this);
  a2_.SetDelegate(this);
  iir_dist_thresh_.SetDelegate(this);
}

void IirFilterInterpreter::SyncInterpretImpl(HardwareState& hwstate,
                                             stime_t* timeout) {
  const char name[] = "IirFilterInterpreter::SyncInterpretImpl";
  LogHardwareStatePre(name, hwstate);

  // Delete old entries from map
  std::vector<short> dead_ids;
  dead_ids.reserve(histories_.size());

  for (std::map<short, IoHistory>::iterator it = histories_.begin(),
           e = histories_.end(); it != e; ++it)
    if (!hwstate.GetFingerState((*it).first))
      dead_ids.push_back((*it).first);
  for (auto dead_id : dead_ids)
    histories_.erase(dead_id);

  // Modify current hwstate
  for (size_t i = 0; i < hwstate.finger_cnt; i++) {
    FingerState& fs = hwstate.fingers[i];
    std::map<short, IoHistory>::iterator history =
        histories_.find(fs.tracking_id);
    if (history == histories_.end()) {
      // new finger
      IoHistory hist(fs);
      histories_[fs.tracking_id] = hist;
      continue;
    }
    // existing finger, apply filter
    IoHistory* hist = &(*history).second;

    // Finger WARP detected, adjust the IO history
    if (adjust_iir_on_warp_.val_) {
      float dx = 0.0, dy = 0.0;

      if (fs.flags & GESTURES_FINGER_WARP_X_MOVE)
        dx = fs.position_x - hist->PrevIn(0)->position_x;
      if (fs.flags & GESTURES_FINGER_WARP_Y_MOVE)
        dy = fs.position_y - hist->PrevIn(0)->position_y;

      hist->WarpBy(dx, dy);
    }

    float dx = fs.position_x - hist->PrevOut(0)->position_x;
    float dy = fs.position_y - hist->PrevOut(0)->position_y;

    // IIR filter is too smooth for a quick finger movement. We do a simple
    // rolling average if the position change between current and previous
    // frames is larger than iir_dist_thresh_.
    if (dx * dx + dy * dy > iir_dist_thresh_.val_ * iir_dist_thresh_.val_)
      using_iir_ = false;
    else
      using_iir_ = true;

    // TODO(adlr): consider applying filter to other fields
    float FingerState::*fields[] = { &FingerState::position_x,
                                     &FingerState::position_y,
                                     &FingerState::pressure };
    for (size_t f_idx = 0; f_idx < arraysize(fields); f_idx++) {
      float FingerState::*field = fields[f_idx];
      // Keep the current pressure reading, so we could make sure the pressure
      // values will be same if there is two fingers on a SemiMT device.
      if (hwprops_ && hwprops_->support_semi_mt &&
          (field == &FingerState::pressure)) {
        hist->NextOut()->pressure = fs.pressure;
        continue;
      }

      if (adjust_iir_on_warp_.val_) {
        if (field == &FingerState::position_x &&
            (fs.flags & GESTURES_FINGER_WARP_X_MOVE)) {
          hist->NextOut()->position_x = fs.position_x;
          continue;
        }

        if (field == &FingerState::position_y &&
            (fs.flags & GESTURES_FINGER_WARP_Y_MOVE)) {
          hist->NextOut()->position_y = fs.position_y;
          continue;
        }
      }

      if (using_iir_) {
        hist->NextOut()->*field =
            b3_.val_ * hist->PrevIn(2)->*field +
            b2_.val_ * hist->PrevIn(1)->*field +
            b1_.val_ * hist->PrevIn(0)->*field +
            b0_.val_ * fs.*field -
            a2_.val_ * hist->PrevOut(1)->*field -
            a1_.val_ * hist->PrevOut(0)->*field;
      } else {
        hist->NextOut()->*field = 0.5 * (fs.*field + hist->PrevOut(0)->*field);
      }
    }
    float FingerState::*pass_fields[] = { &FingerState::touch_major,
                                          &FingerState::touch_minor,
                                          &FingerState::width_major,
                                          &FingerState::width_minor,
                                          &FingerState::orientation };
    for (size_t f_idx = 0; f_idx < arraysize(pass_fields); f_idx++)
      hist->NextOut()->*pass_fields[f_idx] = fs.*pass_fields[f_idx];
    hist->NextOut()->flags = fs.flags;
    *hist->NextIn() = fs;
    fs = *hist->NextOut();
    hist->Increment();
  }
  LogHardwareStatePost(name, hwstate);
  next_->SyncInterpret(hwstate, timeout);
}

void IirFilterInterpreter::DoubleWasWritten(DoubleProperty* prop) {
  histories_.clear();
}

void IirFilterInterpreter::IoHistory::WarpBy(float dx, float dy) {
  for (size_t i = 0; i < kInSize; i++) {
    PrevIn(i)->position_x += dx;
    PrevIn(i)->position_y += dy;
  }

  for (size_t i = 0; i < kOutSize; i++) {
    PrevOut(i)->position_x += dx;
    PrevOut(i)->position_y += dy;
  }
}

}  // namespace gestures