#include "dtw.h"

void computeEnvelope(const vector<floattype> &array, uint constraint,
                     vector<floattype> &maxvalues,
                     vector<floattype> &minvalues) {
  uint width = 1 + 2 * constraint;
  deque<int> maxfifo, minfifo;
  maxfifo.push_back(0);
  minfifo.push_back(0);
  for (uint i = 1; i < array.size(); ++i) {
    if (i >= constraint + 1) {
      maxvalues[i - constraint - 1] = array[maxfifo.front()];
      minvalues[i - constraint - 1] = array[minfifo.front()];
    }
    if (array[i] > array[i - 1]) {  // overshoot
      maxfifo.pop_back();
      while (maxfifo.size() > 0) {
        if (array[i] <= array[maxfifo.back()]) break;
        maxfifo.pop_back();
      }
    } else {
      minfifo.pop_back();
      while (minfifo.size() > 0) {
        if (array[i] >= array[minfifo.back()]) break;
        minfifo.pop_back();
      }
    }
    maxfifo.push_back(i);
    minfifo.push_back(i);
    if (i == width + maxfifo.front())
      maxfifo.pop_front();
    else if (i == width + minfifo.front())
      minfifo.pop_front();
  }
  for (uint i = array.size(); i <= array.size() + constraint; ++i) {
    if (i >= constraint + 1) {
      maxvalues[i - constraint - 1] = array[maxfifo.front()];
      minvalues[i - constraint - 1] = array[minfifo.front()];
    }
    if (i - maxfifo.front() >= width) maxfifo.pop_front();
    if (i - minfifo.front() >= width) minfifo.pop_front();
  }
}

dtw::dtw(uint n, uint constraint)
    : mGamma(n, vector<double>(n, INF)), mN(n), mConstraint(constraint) {}

double dtw::fastdynamic(const vector<double> &v, const vector<double> &w) {
  if (!fast) return dynamic(v, w, mConstraint, 1);
  assert(static_cast<int>(v.size()) == mN);
  assert(static_cast<int>(w.size()) == mN);
  assert(static_cast<int>(mGamma.size()) == mN);
  double Best(INF);
  for (int i = 0; i < mN; ++i) {
    assert(static_cast<int>(mGamma[i].size()) == mN);
    for (int j = max(0, i - mConstraint); j < min(mN, i + mConstraint + 1);
         ++j) {
      Best = INF;
      if (i > 0) Best = mGamma[i - 1][j];
      if (j > 0) Best = min(Best, mGamma[i][j - 1]);
      if ((i > 0) && (j > 0)) Best = min(Best, mGamma[i - 1][j - 1]);
      if ((i == 0) && (j == 0))
        mGamma[i][j] = fabs(v[i] - w[j]);
      else
        mGamma[i][j] = Best + fabs(v[i] - w[j]);
    }
  }
  return mGamma[mN - 1][mN - 1];
}

double dtw::dynamic(const vector<double> &v, const vector<double> &w,
                    int constraint, int p) {
  assert(v.size() == w.size());
  int n(v.size());
  vector<vector<double>> gamma(n, vector<double>(n, 0.0));
  for (int i = 0; i < n; ++i) {
    for (int j = 0; j < n; ++j) {
      if (abs(i - j) > constraint) {
        gamma[i][j] = INF;
        continue;
      }
      vector<double> previous(0);
      if (i > 0) previous.push_back(gamma[i - 1][j]);
      if (j > 0) previous.push_back(gamma[i][j - 1]);
      if ((i > 0) && (j > 0)) previous.push_back(gamma[i - 1][j - 1]);
      double smallest = *min_element(previous.begin(), previous.end());
      if (p != INF)
        gamma[i][j] = pow(fabs(v[i] - w[j]), p) + smallest;
      else
        gamma[i][j] = max(fabs(v[i] - w[j]), smallest);
    }
  }
  if (p != INF)
    return pow(gamma[n - 1][n - 1], 1.0 / p);
  else
    return gamma[n - 1][n - 1];
}

NearestNeighbor::NearestNeighbor(const vector<double> &v, int constraint)
    : mDTW(v.size(), constraint) {}

NaiveNearestNeighbor::NaiveNearestNeighbor(const vector<double> &v,
                                           int constraint)
    : NearestNeighbor(v, constraint),
      lb_keogh(0),
      full_dtw(0),
      V(v),
      bestsofar(dtw::INF) {}

double NaiveNearestNeighbor::test(const vector<double> &candidate) {
  ++lb_keogh;
  ++full_dtw;
  const double trueerror = mDTW.fastdynamic(V, candidate);  //,mConstraint,1);
  if (trueerror < bestsofar) bestsofar = trueerror;
  return bestsofar;
}

void NaiveNearestNeighbor::resetStatistics() {
  lb_keogh = 0;
  full_dtw = 0;
}

double NaiveNearestNeighbor::getLowestCost() { return bestsofar; }

int NaiveNearestNeighbor::getNumberOfDTW() { return full_dtw; }

int NaiveNearestNeighbor::getNumberOfCandidates() { return lb_keogh; }