/*
 * Copyright (C) 2012 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#define LOG_TAG "VelocityTracker"

#include <android-base/logging.h>
#include <ftl/enum.h>
#include <inttypes.h>
#include <limits.h>
#include <math.h>
#include <array>
#include <optional>

#include <input/PrintTools.h>
#include <input/VelocityTracker.h>
#include <utils/BitSet.h>
#include <utils/Timers.h>

using std::literals::chrono_literals::operator""ms;

namespace android {

/**
 * Log debug messages about velocity tracking.
 * Enable this via "adb shell setprop log.tag.VelocityTrackerVelocity DEBUG" (requires restart)
 */
const bool DEBUG_VELOCITY =
        __android_log_is_loggable(ANDROID_LOG_DEBUG, LOG_TAG "Velocity", ANDROID_LOG_INFO);

/**
 * Log debug messages about the progress of the algorithm itself.
 * Enable this via "adb shell setprop log.tag.VelocityTrackerStrategy DEBUG" (requires restart)
 */
const bool DEBUG_STRATEGY =
        __android_log_is_loggable(ANDROID_LOG_DEBUG, LOG_TAG "Strategy", ANDROID_LOG_INFO);

/**
 * Log debug messages about the 'impulse' strategy.
 * Enable this via "adb shell setprop log.tag.VelocityTrackerImpulse DEBUG" (requires restart)
 */
const bool DEBUG_IMPULSE =
        __android_log_is_loggable(ANDROID_LOG_DEBUG, LOG_TAG "Impulse", ANDROID_LOG_INFO);

// Nanoseconds per milliseconds.
static const nsecs_t NANOS_PER_MS = 1000000;

// Seconds per nanosecond.
static const float SECONDS_PER_NANO = 1E-9;

// All axes supported for velocity tracking, mapped to their default strategies.
// Although other strategies are available for testing and comparison purposes,
// the default strategy is the one that applications will actually use.  Be very careful
// when adjusting the default strategy because it can dramatically affect
// (often in a bad way) the user experience.
static const std::map<int32_t, VelocityTracker::Strategy> DEFAULT_STRATEGY_BY_AXIS =
        {{AMOTION_EVENT_AXIS_X, VelocityTracker::Strategy::LSQ2},
         {AMOTION_EVENT_AXIS_Y, VelocityTracker::Strategy::LSQ2},
         {AMOTION_EVENT_AXIS_SCROLL, VelocityTracker::Strategy::IMPULSE}};

// Axes specifying location on a 2D plane (i.e. X and Y).
static const std::set<int32_t> PLANAR_AXES = {AMOTION_EVENT_AXIS_X, AMOTION_EVENT_AXIS_Y};

// Axes whose motion values are differential values (i.e. deltas).
static const std::set<int32_t> DIFFERENTIAL_AXES = {AMOTION_EVENT_AXIS_SCROLL};

// Threshold for determining that a pointer has stopped moving.
// Some input devices do not send ACTION_MOVE events in the case where a pointer has
// stopped.  We need to detect this case so that we can accurately predict the
// velocity after the pointer starts moving again.
static const std::chrono::duration ASSUME_POINTER_STOPPED_TIME = 40ms;

static std::string toString(std::chrono::nanoseconds t) {
    std::stringstream stream;
    stream.precision(1);
    stream << std::fixed << std::chrono::duration<float, std::milli>(t).count() << " ms";
    return stream.str();
}

static float vectorDot(const float* a, const float* b, uint32_t m) {
    float r = 0;
    for (size_t i = 0; i < m; i++) {
        r += *(a++) * *(b++);
    }
    return r;
}

static float vectorNorm(const float* a, uint32_t m) {
    float r = 0;
    for (size_t i = 0; i < m; i++) {
        float t = *(a++);
        r += t * t;
    }
    return sqrtf(r);
}

static std::string vectorToString(const float* a, uint32_t m) {
    std::string str;
    str += "[";
    for (size_t i = 0; i < m; i++) {
        if (i) {
            str += ",";
        }
        str += android::base::StringPrintf(" %f", *(a++));
    }
    str += " ]";
    return str;
}

static std::string vectorToString(const std::vector<float>& v) {
    return vectorToString(v.data(), v.size());
}

static std::string matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) {
    std::string str;
    str = "[";
    for (size_t i = 0; i < m; i++) {
        if (i) {
            str += ",";
        }
        str += " [";
        for (size_t j = 0; j < n; j++) {
            if (j) {
                str += ",";
            }
            str += android::base::StringPrintf(" %f", a[rowMajor ? i * n + j : j * m + i]);
        }
        str += " ]";
    }
    str += " ]";
    return str;
}


// --- VelocityTracker ---

VelocityTracker::VelocityTracker(const Strategy strategy)
      : mLastEventTime(0), mCurrentPointerIdBits(0), mOverrideStrategy(strategy) {}

bool VelocityTracker::isAxisSupported(int32_t axis) {
    return DEFAULT_STRATEGY_BY_AXIS.find(axis) != DEFAULT_STRATEGY_BY_AXIS.end();
}

void VelocityTracker::configureStrategy(int32_t axis) {
    const bool isDifferentialAxis = DIFFERENTIAL_AXES.find(axis) != DIFFERENTIAL_AXES.end();
    if (isDifferentialAxis || mOverrideStrategy == VelocityTracker::Strategy::DEFAULT) {
        // Do not allow overrides of strategies for differential axes, for now.
        mConfiguredStrategies[axis] = createStrategy(DEFAULT_STRATEGY_BY_AXIS.at(axis),
                                                     /*deltaValues=*/isDifferentialAxis);
    } else {
        mConfiguredStrategies[axis] = createStrategy(mOverrideStrategy, /*deltaValues=*/false);
    }
}

std::unique_ptr<VelocityTrackerStrategy> VelocityTracker::createStrategy(
        VelocityTracker::Strategy strategy, bool deltaValues) {
    switch (strategy) {
        case VelocityTracker::Strategy::IMPULSE:
            ALOGI_IF(DEBUG_STRATEGY, "Initializing impulse strategy");
            return std::make_unique<ImpulseVelocityTrackerStrategy>(deltaValues);

        case VelocityTracker::Strategy::LSQ1:
            return std::make_unique<LeastSquaresVelocityTrackerStrategy>(1);

        case VelocityTracker::Strategy::LSQ2:
            ALOGI_IF(DEBUG_STRATEGY && !DEBUG_IMPULSE, "Initializing lsq2 strategy");
            return std::make_unique<LeastSquaresVelocityTrackerStrategy>(2);

        case VelocityTracker::Strategy::LSQ3:
            return std::make_unique<LeastSquaresVelocityTrackerStrategy>(3);

        case VelocityTracker::Strategy::WLSQ2_DELTA:
            return std::make_unique<
                    LeastSquaresVelocityTrackerStrategy>(2,
                                                         LeastSquaresVelocityTrackerStrategy::
                                                                 Weighting::DELTA);
        case VelocityTracker::Strategy::WLSQ2_CENTRAL:
            return std::make_unique<
                    LeastSquaresVelocityTrackerStrategy>(2,
                                                         LeastSquaresVelocityTrackerStrategy::
                                                                 Weighting::CENTRAL);
        case VelocityTracker::Strategy::WLSQ2_RECENT:
            return std::make_unique<
                    LeastSquaresVelocityTrackerStrategy>(2,
                                                         LeastSquaresVelocityTrackerStrategy::
                                                                 Weighting::RECENT);

        case VelocityTracker::Strategy::INT1:
            return std::make_unique<IntegratingVelocityTrackerStrategy>(1);

        case VelocityTracker::Strategy::INT2:
            return std::make_unique<IntegratingVelocityTrackerStrategy>(2);

        case VelocityTracker::Strategy::LEGACY:
            return std::make_unique<LegacyVelocityTrackerStrategy>();

        default:
            break;
    }
    LOG(FATAL) << "Invalid strategy: " << ftl::enum_string(strategy)
               << ", deltaValues = " << deltaValues;

    return nullptr;
}

void VelocityTracker::clear() {
    mCurrentPointerIdBits.clear();
    mActivePointerId = std::nullopt;
    mConfiguredStrategies.clear();
}

void VelocityTracker::clearPointer(int32_t pointerId) {
    mCurrentPointerIdBits.clearBit(pointerId);

    if (mActivePointerId && *mActivePointerId == pointerId) {
        // The active pointer id is being removed. Mark it invalid and try to find a new one
        // from the remaining pointers.
        mActivePointerId = std::nullopt;
        if (!mCurrentPointerIdBits.isEmpty()) {
            mActivePointerId = mCurrentPointerIdBits.firstMarkedBit();
        }
    }

    for (const auto& [_, strategy] : mConfiguredStrategies) {
        strategy->clearPointer(pointerId);
    }
}

void VelocityTracker::addMovement(nsecs_t eventTime, int32_t pointerId, int32_t axis,
                                  float position) {
    if (pointerId < 0 || pointerId > MAX_POINTER_ID) {
        LOG(FATAL) << "Invalid pointer ID " << pointerId << " for axis "
                   << MotionEvent::getLabel(axis);
    }

    if (mCurrentPointerIdBits.hasBit(pointerId) &&
        std::chrono::nanoseconds(eventTime - mLastEventTime) > ASSUME_POINTER_STOPPED_TIME) {
        ALOGD_IF(DEBUG_VELOCITY, "VelocityTracker: stopped for %s, clearing state.",
                 toString(std::chrono::nanoseconds(eventTime - mLastEventTime)).c_str());

        // We have not received any movements for too long.  Assume that all pointers
        // have stopped.
        mConfiguredStrategies.clear();
    }
    mLastEventTime = eventTime;

    mCurrentPointerIdBits.markBit(pointerId);
    if (!mActivePointerId) {
        // Let this be the new active pointer if no active pointer is currently set
        mActivePointerId = pointerId;
    }

    if (mConfiguredStrategies.find(axis) == mConfiguredStrategies.end()) {
        configureStrategy(axis);
    }
    mConfiguredStrategies[axis]->addMovement(eventTime, pointerId, position);

    if (DEBUG_VELOCITY) {
        LOG(INFO) << "VelocityTracker: addMovement axis=" << MotionEvent::getLabel(axis)
                  << ", eventTime=" << eventTime << ", pointerId=" << pointerId
                  << ", activePointerId=" << toString(mActivePointerId) << ", position=" << position
                  << ", velocity=" << toString(getVelocity(axis, pointerId));
    }
}

void VelocityTracker::addMovement(const MotionEvent& event) {
    // Stores data about which axes to process based on the incoming motion event.
    std::set<int32_t> axesToProcess;
    int32_t actionMasked = event.getActionMasked();

    switch (actionMasked) {
        case AMOTION_EVENT_ACTION_DOWN:
        case AMOTION_EVENT_ACTION_HOVER_ENTER:
            // Clear all pointers on down before adding the new movement.
            clear();
            axesToProcess.insert(PLANAR_AXES.begin(), PLANAR_AXES.end());
            break;
        case AMOTION_EVENT_ACTION_POINTER_DOWN: {
            // Start a new movement trace for a pointer that just went down.
            // We do this on down instead of on up because the client may want to query the
            // final velocity for a pointer that just went up.
            clearPointer(event.getPointerId(event.getActionIndex()));
            axesToProcess.insert(PLANAR_AXES.begin(), PLANAR_AXES.end());
            break;
        }
        case AMOTION_EVENT_ACTION_MOVE:
        case AMOTION_EVENT_ACTION_HOVER_MOVE:
            axesToProcess.insert(PLANAR_AXES.begin(), PLANAR_AXES.end());
            break;
        case AMOTION_EVENT_ACTION_POINTER_UP:
            if (event.getFlags() & AMOTION_EVENT_FLAG_CANCELED) {
                clearPointer(event.getPointerId(event.getActionIndex()));
                return;
            }
            // Continue to ACTION_UP to ensure that the POINTER_STOPPED logic is triggered.
            [[fallthrough]];
        case AMOTION_EVENT_ACTION_UP: {
            std::chrono::nanoseconds delaySinceLastEvent(event.getEventTime() - mLastEventTime);
            if (delaySinceLastEvent > ASSUME_POINTER_STOPPED_TIME) {
                ALOGD_IF(DEBUG_VELOCITY,
                         "VelocityTracker: stopped for %s, clearing state upon pointer liftoff.",
                         toString(delaySinceLastEvent).c_str());
                // We have not received any movements for too long.  Assume that all pointers
                // have stopped.
                for (int32_t axis : PLANAR_AXES) {
                    mConfiguredStrategies.erase(axis);
                }
            }
            // These actions because they do not convey any new information about
            // pointer movement.  We also want to preserve the last known velocity of the pointers.
            // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position
            // of the pointers that went up.  ACTION_POINTER_UP does include the new position of
            // pointers that remained down but we will also receive an ACTION_MOVE with this
            // information if any of them actually moved.  Since we don't know how many pointers
            // will be going up at once it makes sense to just wait for the following ACTION_MOVE
            // before adding the movement.
            return;
        }
        case AMOTION_EVENT_ACTION_SCROLL:
            axesToProcess.insert(AMOTION_EVENT_AXIS_SCROLL);
            break;
        case AMOTION_EVENT_ACTION_CANCEL: {
            clear();
            return;
        }

        default:
            // Ignore all other actions.
            return;
    }

    const size_t historySize = event.getHistorySize();
    for (size_t h = 0; h <= historySize; h++) {
        const nsecs_t eventTime = event.getHistoricalEventTime(h);
        for (size_t i = 0; i < event.getPointerCount(); i++) {
            if (event.isResampled(i, h)) {
                continue; // skip resampled samples
            }
            const int32_t pointerId = event.getPointerId(i);
            for (int32_t axis : axesToProcess) {
                const float position = event.getHistoricalAxisValue(axis, i, h);
                addMovement(eventTime, pointerId, axis, position);
            }
        }
    }
}

std::optional<float> VelocityTracker::getVelocity(int32_t axis, int32_t pointerId) const {
    const auto& it = mConfiguredStrategies.find(axis);
    if (it != mConfiguredStrategies.end()) {
        return it->second->getVelocity(pointerId);
    }
    return {};
}

VelocityTracker::ComputedVelocity VelocityTracker::getComputedVelocity(int32_t units,
                                                                       float maxVelocity) {
    ComputedVelocity computedVelocity;
    for (const auto& [axis, _] : mConfiguredStrategies) {
        BitSet32 copyIdBits = BitSet32(mCurrentPointerIdBits);
        while (!copyIdBits.isEmpty()) {
            uint32_t id = copyIdBits.clearFirstMarkedBit();
            std::optional<float> velocity = getVelocity(axis, id);
            if (velocity) {
                float adjustedVelocity =
                        std::clamp(*velocity * units / 1000, -maxVelocity, maxVelocity);
                computedVelocity.addVelocity(axis, id, adjustedVelocity);
            }
        }
    }
    return computedVelocity;
}

AccumulatingVelocityTrackerStrategy::AccumulatingVelocityTrackerStrategy(
        nsecs_t horizonNanos, bool maintainHorizonDuringAdd)
      : mHorizonNanos(horizonNanos), mMaintainHorizonDuringAdd(maintainHorizonDuringAdd) {}

void AccumulatingVelocityTrackerStrategy::clearPointer(int32_t pointerId) {
    mMovements.erase(pointerId);
}

void AccumulatingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, int32_t pointerId,
                                                      float position) {
    auto [ringBufferIt, _] = mMovements.try_emplace(pointerId, HISTORY_SIZE);
    RingBuffer<Movement>& movements = ringBufferIt->second;
    const size_t size = movements.size();

    if (size != 0 && movements[size - 1].eventTime == eventTime) {
        // When ACTION_POINTER_DOWN happens, we will first receive ACTION_MOVE with the coordinates
        // of the existing pointers, and then ACTION_POINTER_DOWN with the coordinates that include
        // the new pointer. If the eventtimes for both events are identical, just update the data
        // for this time (i.e. pop out the last element, and insert the updated movement).
        // We only compare against the last value, as it is likely that addMovement is called
        // in chronological order as events occur.
        movements.popBack();
    }

    movements.pushBack({eventTime, position});

    // Clear movements that do not fall within `mHorizonNanos` of the latest movement.
    // Note that, if in the future we decide to use more movements (i.e. increase HISTORY_SIZE),
    // we can consider making this step binary-search based, which will give us some improvement.
    if (mMaintainHorizonDuringAdd) {
        while (eventTime - movements[0].eventTime > mHorizonNanos) {
            movements.popFront();
        }
    }
}

// --- LeastSquaresVelocityTrackerStrategy ---

LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy(uint32_t degree,
                                                                         Weighting weighting)
      : AccumulatingVelocityTrackerStrategy(HORIZON /*horizonNanos*/,
                                            true /*maintainHorizonDuringAdd*/),
        mDegree(degree),
        mWeighting(weighting) {}

LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() {}

/**
 * Solves a linear least squares problem to obtain a N degree polynomial that fits
 * the specified input data as nearly as possible.
 *
 * Returns true if a solution is found, false otherwise.
 *
 * The input consists of two vectors of data points X and Y with indices 0..m-1
 * along with a weight vector W of the same size.
 *
 * The output is a vector B with indices 0..n that describes a polynomial
 * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i]
 * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized.
 *
 * Accordingly, the weight vector W should be initialized by the caller with the
 * reciprocal square root of the variance of the error in each input data point.
 * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]).
 * The weights express the relative importance of each data point.  If the weights are
 * all 1, then the data points are considered to be of equal importance when fitting
 * the polynomial.  It is a good idea to choose weights that diminish the importance
 * of data points that may have higher than usual error margins.
 *
 * Errors among data points are assumed to be independent.  W is represented here
 * as a vector although in the literature it is typically taken to be a diagonal matrix.
 *
 * That is to say, the function that generated the input data can be approximated
 * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n.
 *
 * The coefficient of determination (R^2) is also returned to describe the goodness
 * of fit of the model for the given data.  It is a value between 0 and 1, where 1
 * indicates perfect correspondence.
 *
 * This function first expands the X vector to a m by n matrix A such that
 * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then
 * multiplies it by w[i]./
 *
 * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q
 * and an m by n upper triangular matrix R.  Because R is upper triangular (lower
 * part is all zeroes), we can simplify the decomposition into an m by n matrix
 * Q1 and a n by n matrix R1 such that A = Q1 R1.
 *
 * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y)
 * to find B.
 *
 * For efficiency, we lay out A and Q column-wise in memory because we frequently
 * operate on the column vectors.  Conversely, we lay out R row-wise.
 *
 * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares
 * http://en.wikipedia.org/wiki/Gram-Schmidt
 */
static std::optional<float> solveLeastSquares(const std::vector<float>& x,
                                              const std::vector<float>& y,
                                              const std::vector<float>& w, uint32_t n) {
    const size_t m = x.size();

    ALOGD_IF(DEBUG_STRATEGY, "solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n),
             vectorToString(x).c_str(), vectorToString(y).c_str(), vectorToString(w).c_str());

    LOG_ALWAYS_FATAL_IF(m != y.size() || m != w.size(), "Mismatched vector sizes");

    // Expand the X vector to a matrix A, pre-multiplied by the weights.
    float a[n][m]; // column-major order
    for (uint32_t h = 0; h < m; h++) {
        a[0][h] = w[h];
        for (uint32_t i = 1; i < n; i++) {
            a[i][h] = a[i - 1][h] * x[h];
        }
    }

    ALOGD_IF(DEBUG_STRATEGY, "  - a=%s",
             matrixToString(&a[0][0], m, n, /*rowMajor=*/false).c_str());

    // Apply the Gram-Schmidt process to A to obtain its QR decomposition.
    float q[n][m]; // orthonormal basis, column-major order
    float r[n][n]; // upper triangular matrix, row-major order
    for (uint32_t j = 0; j < n; j++) {
        for (uint32_t h = 0; h < m; h++) {
            q[j][h] = a[j][h];
        }
        for (uint32_t i = 0; i < j; i++) {
            float dot = vectorDot(&q[j][0], &q[i][0], m);
            for (uint32_t h = 0; h < m; h++) {
                q[j][h] -= dot * q[i][h];
            }
        }

        float norm = vectorNorm(&q[j][0], m);
        if (norm < 0.000001f) {
            // vectors are linearly dependent or zero so no solution
            ALOGD_IF(DEBUG_STRATEGY, "  - no solution, norm=%f", norm);
            return {};
        }

        float invNorm = 1.0f / norm;
        for (uint32_t h = 0; h < m; h++) {
            q[j][h] *= invNorm;
        }
        for (uint32_t i = 0; i < n; i++) {
            r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m);
        }
    }
    if (DEBUG_STRATEGY) {
        ALOGD("  - q=%s", matrixToString(&q[0][0], m, n, /*rowMajor=*/false).c_str());
        ALOGD("  - r=%s", matrixToString(&r[0][0], n, n, /*rowMajor=*/true).c_str());

        // calculate QR, if we factored A correctly then QR should equal A
        float qr[n][m];
        for (uint32_t h = 0; h < m; h++) {
            for (uint32_t i = 0; i < n; i++) {
                qr[i][h] = 0;
                for (uint32_t j = 0; j < n; j++) {
                    qr[i][h] += q[j][h] * r[j][i];
                }
            }
        }
        ALOGD("  - qr=%s", matrixToString(&qr[0][0], m, n, /*rowMajor=*/false).c_str());
    }

    // Solve R B = Qt W Y to find B.  This is easy because R is upper triangular.
    // We just work from bottom-right to top-left calculating B's coefficients.
    float wy[m];
    for (uint32_t h = 0; h < m; h++) {
        wy[h] = y[h] * w[h];
    }
    std::array<float, VelocityTracker::MAX_DEGREE + 1> outB;
    for (uint32_t i = n; i != 0; ) {
        i--;
        outB[i] = vectorDot(&q[i][0], wy, m);
        for (uint32_t j = n - 1; j > i; j--) {
            outB[i] -= r[i][j] * outB[j];
        }
        outB[i] /= r[i][i];
    }

    ALOGD_IF(DEBUG_STRATEGY, "  - b=%s", vectorToString(outB.data(), n).c_str());

    // Calculate the coefficient of determination as 1 - (SSerr / SStot) where
    // SSerr is the residual sum of squares (variance of the error),
    // and SStot is the total sum of squares (variance of the data) where each
    // has been weighted.
    float ymean = 0;
    for (uint32_t h = 0; h < m; h++) {
        ymean += y[h];
    }
    ymean /= m;

    if (DEBUG_STRATEGY) {
        float sserr = 0;
        float sstot = 0;
        for (uint32_t h = 0; h < m; h++) {
            float err = y[h] - outB[0];
            float term = 1;
            for (uint32_t i = 1; i < n; i++) {
                term *= x[h];
                err -= term * outB[i];
            }
            sserr += w[h] * w[h] * err * err;
            float var = y[h] - ymean;
            sstot += w[h] * w[h] * var * var;
        }
        ALOGD("  - sserr=%f", sserr);
        ALOGD("  - sstot=%f", sstot);
    }

    return outB[1];
}

/*
 * Optimized unweighted second-order least squares fit. About 2x speed improvement compared to
 * the default implementation
 */
std::optional<float> LeastSquaresVelocityTrackerStrategy::solveUnweightedLeastSquaresDeg2(
        const RingBuffer<Movement>& movements) const {
    // Solving y = a*x^2 + b*x + c, where
    //      - "x" is age (i.e. duration since latest movement) of the movemnets
    //      - "y" is positions of the movements.
    float sxi = 0, sxiyi = 0, syi = 0, sxi2 = 0, sxi3 = 0, sxi2yi = 0, sxi4 = 0;

    const size_t count = movements.size();
    const Movement& newestMovement = movements[count - 1];
    for (size_t i = 0; i < count; i++) {
        const Movement& movement = movements[i];
        nsecs_t age = newestMovement.eventTime - movement.eventTime;
        float xi = -age * SECONDS_PER_NANO;
        float yi = movement.position;

        float xi2 = xi*xi;
        float xi3 = xi2*xi;
        float xi4 = xi3*xi;
        float xiyi = xi*yi;
        float xi2yi = xi2*yi;

        sxi += xi;
        sxi2 += xi2;
        sxiyi += xiyi;
        sxi2yi += xi2yi;
        syi += yi;
        sxi3 += xi3;
        sxi4 += xi4;
    }

    float Sxx = sxi2 - sxi*sxi / count;
    float Sxy = sxiyi - sxi*syi / count;
    float Sxx2 = sxi3 - sxi*sxi2 / count;
    float Sx2y = sxi2yi - sxi2*syi / count;
    float Sx2x2 = sxi4 - sxi2*sxi2 / count;

    float denominator = Sxx*Sx2x2 - Sxx2*Sxx2;
    if (denominator == 0) {
        ALOGW("division by 0 when computing velocity, Sxx=%f, Sx2x2=%f, Sxx2=%f", Sxx, Sx2x2, Sxx2);
        return std::nullopt;
    }

    return (Sxy * Sx2x2 - Sx2y * Sxx2) / denominator;
}

std::optional<float> LeastSquaresVelocityTrackerStrategy::getVelocity(int32_t pointerId) const {
    const auto movementIt = mMovements.find(pointerId);
    if (movementIt == mMovements.end()) {
        return std::nullopt; // no data
    }

    const RingBuffer<Movement>& movements = movementIt->second;
    const size_t size = movements.size();
    if (size == 0) {
        return std::nullopt; // no data
    }

    uint32_t degree = mDegree;
    if (degree > size - 1) {
        degree = size - 1;
    }

    if (degree <= 0) {
        return std::nullopt;
    }

    if (degree == 2 && mWeighting == Weighting::NONE) {
        // Optimize unweighted, quadratic polynomial fit
        return solveUnweightedLeastSquaresDeg2(movements);
    }

    // Iterate over movement samples in reverse time order and collect samples.
    std::vector<float> positions;
    std::vector<float> w;
    std::vector<float> time;

    const Movement& newestMovement = movements[size - 1];
    for (ssize_t i = size - 1; i >= 0; i--) {
        const Movement& movement = movements[i];
        nsecs_t age = newestMovement.eventTime - movement.eventTime;
        positions.push_back(movement.position);
        w.push_back(chooseWeight(pointerId, i));
        time.push_back(-age * 0.000000001f);
    }

    // General case for an Nth degree polynomial fit
    return solveLeastSquares(time, positions, w, degree + 1);
}

float LeastSquaresVelocityTrackerStrategy::chooseWeight(int32_t pointerId, uint32_t index) const {
    const RingBuffer<Movement>& movements = mMovements.at(pointerId);
    const size_t size = movements.size();
    switch (mWeighting) {
        case Weighting::DELTA: {
            // Weight points based on how much time elapsed between them and the next
            // point so that points that "cover" a shorter time span are weighed less.
            //   delta  0ms: 0.5
            //   delta 10ms: 1.0
            if (index == size - 1) {
                return 1.0f;
            }
            float deltaMillis =
                    (movements[index + 1].eventTime - movements[index].eventTime) * 0.000001f;
            if (deltaMillis < 0) {
                return 0.5f;
            }
            if (deltaMillis < 10) {
                return 0.5f + deltaMillis * 0.05;
            }
            return 1.0f;
        }

        case Weighting::CENTRAL: {
            // Weight points based on their age, weighing very recent and very old points less.
            //   age  0ms: 0.5
            //   age 10ms: 1.0
            //   age 50ms: 1.0
            //   age 60ms: 0.5
            float ageMillis =
                    (movements[size - 1].eventTime - movements[index].eventTime) * 0.000001f;
            if (ageMillis < 0) {
                return 0.5f;
            }
            if (ageMillis < 10) {
                return 0.5f + ageMillis * 0.05;
            }
            if (ageMillis < 50) {
                return 1.0f;
            }
            if (ageMillis < 60) {
                return 0.5f + (60 - ageMillis) * 0.05;
            }
            return 0.5f;
        }

        case Weighting::RECENT: {
            // Weight points based on their age, weighing older points less.
            //   age   0ms: 1.0
            //   age  50ms: 1.0
            //   age 100ms: 0.5
            float ageMillis =
                    (movements[size - 1].eventTime - movements[index].eventTime) * 0.000001f;
            if (ageMillis < 50) {
                return 1.0f;
            }
            if (ageMillis < 100) {
                return 0.5f + (100 - ageMillis) * 0.01f;
            }
            return 0.5f;
        }

        case Weighting::NONE:
            return 1.0f;
    }
}

// --- IntegratingVelocityTrackerStrategy ---

IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) :
        mDegree(degree) {
}

IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() {
}

void IntegratingVelocityTrackerStrategy::clearPointer(int32_t pointerId) {
    mPointerIdBits.clearBit(pointerId);
}

void IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, int32_t pointerId,
                                                     float position) {
    State& state = mPointerState[pointerId];
    if (mPointerIdBits.hasBit(pointerId)) {
        updateState(state, eventTime, position);
    } else {
        initState(state, eventTime, position);
    }

    mPointerIdBits.markBit(pointerId);
}

std::optional<float> IntegratingVelocityTrackerStrategy::getVelocity(int32_t pointerId) const {
    if (mPointerIdBits.hasBit(pointerId)) {
        return mPointerState[pointerId].vel;
    }

    return std::nullopt;
}

void IntegratingVelocityTrackerStrategy::initState(State& state, nsecs_t eventTime,
                                                   float pos) const {
    state.updateTime = eventTime;
    state.degree = 0;

    state.pos = pos;
    state.accel = 0;
    state.vel = 0;
}

void IntegratingVelocityTrackerStrategy::updateState(State& state, nsecs_t eventTime,
                                                     float pos) const {
    const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS;
    const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds

    if (eventTime <= state.updateTime + MIN_TIME_DELTA) {
        return;
    }

    float dt = (eventTime - state.updateTime) * 0.000000001f;
    state.updateTime = eventTime;

    float vel = (pos - state.pos) / dt;
    if (state.degree == 0) {
        state.vel = vel;
        state.degree = 1;
    } else {
        float alpha = dt / (FILTER_TIME_CONSTANT + dt);
        if (mDegree == 1) {
            state.vel += (vel - state.vel) * alpha;
        } else {
            float accel = (vel - state.vel) / dt;
            if (state.degree == 1) {
                state.accel = accel;
                state.degree = 2;
            } else {
                state.accel += (accel - state.accel) * alpha;
            }
            state.vel += (state.accel * dt) * alpha;
        }
    }
    state.pos = pos;
}

// --- LegacyVelocityTrackerStrategy ---

LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy()
      : AccumulatingVelocityTrackerStrategy(HORIZON /*horizonNanos*/,
                                            false /*maintainHorizonDuringAdd*/) {}

LegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() {
}

std::optional<float> LegacyVelocityTrackerStrategy::getVelocity(int32_t pointerId) const {
    const auto movementIt = mMovements.find(pointerId);
    if (movementIt == mMovements.end()) {
        return std::nullopt; // no data
    }

    const RingBuffer<Movement>& movements = movementIt->second;
    const size_t size = movements.size();
    if (size == 0) {
        return std::nullopt; // no data
    }

    const Movement& newestMovement = movements[size - 1];

    // Find the oldest sample that contains the pointer and that is not older than HORIZON.
    nsecs_t minTime = newestMovement.eventTime - HORIZON;
    uint32_t oldestIndex = size - 1;
    for (ssize_t i = size - 1; i >= 0; i--) {
        const Movement& nextOldestMovement = movements[i];
        if (nextOldestMovement.eventTime < minTime) {
            break;
        }
        oldestIndex = i;
    }

    // Calculate an exponentially weighted moving average of the velocity estimate
    // at different points in time measured relative to the oldest sample.
    // This is essentially an IIR filter.  Newer samples are weighted more heavily
    // than older samples.  Samples at equal time points are weighted more or less
    // equally.
    //
    // One tricky problem is that the sample data may be poorly conditioned.
    // Sometimes samples arrive very close together in time which can cause us to
    // overestimate the velocity at that time point.  Most samples might be measured
    // 16ms apart but some consecutive samples could be only 0.5sm apart because
    // the hardware or driver reports them irregularly or in bursts.
    float accumV = 0;
    uint32_t samplesUsed = 0;
    const Movement& oldestMovement = movements[oldestIndex];
    float oldestPosition = oldestMovement.position;
    nsecs_t lastDuration = 0;

    for (size_t i = oldestIndex; i < size; i++) {
        const Movement& movement = movements[i];
        nsecs_t duration = movement.eventTime - oldestMovement.eventTime;

        // If the duration between samples is small, we may significantly overestimate
        // the velocity.  Consequently, we impose a minimum duration constraint on the
        // samples that we include in the calculation.
        if (duration >= MIN_DURATION) {
            float position = movement.position;
            float scale = 1000000000.0f / duration; // one over time delta in seconds
            float v = (position - oldestPosition) * scale;
            accumV = (accumV * lastDuration + v * duration) / (duration + lastDuration);
            lastDuration = duration;
            samplesUsed += 1;
        }
    }

    if (samplesUsed) {
        return accumV;
    }
    return std::nullopt;
}

// --- ImpulseVelocityTrackerStrategy ---

ImpulseVelocityTrackerStrategy::ImpulseVelocityTrackerStrategy(bool deltaValues)
      : AccumulatingVelocityTrackerStrategy(HORIZON /*horizonNanos*/,
                                            true /*maintainHorizonDuringAdd*/),
        mDeltaValues(deltaValues) {}

ImpulseVelocityTrackerStrategy::~ImpulseVelocityTrackerStrategy() {
}

/**
 * Calculate the total impulse provided to the screen and the resulting velocity.
 *
 * The touchscreen is modeled as a physical object.
 * Initial condition is discussed below, but for now suppose that v(t=0) = 0
 *
 * The kinetic energy of the object at the release is E=0.5*m*v^2
 * Then vfinal = sqrt(2E/m). The goal is to calculate E.
 *
 * The kinetic energy at the release is equal to the total work done on the object by the finger.
 * The total work W is the sum of all dW along the path.
 *
 * dW = F*dx, where dx is the piece of path traveled.
 * Force is change of momentum over time, F = dp/dt = m dv/dt.
 * Then substituting:
 * dW = m (dv/dt) * dx = m * v * dv
 *
 * Summing along the path, we get:
 * W = sum(dW) = sum(m * v * dv) = m * sum(v * dv)
 * Since the mass stays constant, the equation for final velocity is:
 * vfinal = sqrt(2*sum(v * dv))
 *
 * Here,
 * dv : change of velocity = (v[i+1]-v[i])
 * dx : change of distance = (x[i+1]-x[i])
 * dt : change of time = (t[i+1]-t[i])
 * v : instantaneous velocity = dx/dt
 *
 * The final formula is:
 * vfinal = sqrt(2) * sqrt(sum((v[i]-v[i-1])*|v[i]|)) for all i
 * The absolute value is needed to properly account for the sign. If the velocity over a
 * particular segment descreases, then this indicates braking, which means that negative
 * work was done. So for two positive, but decreasing, velocities, this contribution would be
 * negative and will cause a smaller final velocity.
 *
 * Initial condition
 * There are two ways to deal with initial condition:
 * 1) Assume that v(0) = 0, which would mean that the screen is initially at rest.
 * This is not entirely accurate. We are only taking the past X ms of touch data, where X is
 * currently equal to 100. However, a touch event that created a fling probably lasted for longer
 * than that, which would mean that the user has already been interacting with the touchscreen
 * and it has probably already been moving.
 * 2) Assume that the touchscreen has already been moving at a certain velocity, calculate this
 * initial velocity and the equivalent energy, and start with this initial energy.
 * Consider an example where we have the following data, consisting of 3 points:
 *                 time: t0, t1, t2
 *                 x   : x0, x1, x2
 *                 v   : 0 , v1, v2
 * Here is what will happen in each of these scenarios:
 * 1) By directly applying the formula above with the v(0) = 0 boundary condition, we will get
 * vfinal = sqrt(2*(|v1|*(v1-v0) + |v2|*(v2-v1))). This can be simplified since v0=0
 * vfinal = sqrt(2*(|v1|*v1 + |v2|*(v2-v1))) = sqrt(2*(v1^2 + |v2|*(v2 - v1)))
 * since velocity is a real number
 * 2) If we treat the screen as already moving, then it must already have an energy (per mass)
 * equal to 1/2*v1^2. Then the initial energy should be 1/2*v1*2, and only the second segment
 * will contribute to the total kinetic energy (since we can effectively consider that v0=v1).
 * This will give the following expression for the final velocity:
 * vfinal = sqrt(2*(1/2*v1^2 + |v2|*(v2-v1)))
 * This analysis can be generalized to an arbitrary number of samples.
 *
 *
 * Comparing the two equations above, we see that the only mathematical difference
 * is the factor of 1/2 in front of the first velocity term.
 * This boundary condition would allow for the "proper" calculation of the case when all of the
 * samples are equally spaced in time and distance, which should suggest a constant velocity.
 *
 * Note that approach 2) is sensitive to the proper ordering of the data in time, since
 * the boundary condition must be applied to the oldest sample to be accurate.
 */
static float kineticEnergyToVelocity(float work) {
    static constexpr float sqrt2 = 1.41421356237;
    return (work < 0 ? -1.0 : 1.0) * sqrtf(fabsf(work)) * sqrt2;
}

std::optional<float> ImpulseVelocityTrackerStrategy::getVelocity(int32_t pointerId) const {
    const auto movementIt = mMovements.find(pointerId);
    if (movementIt == mMovements.end()) {
        return std::nullopt; // no data
    }

    const RingBuffer<Movement>& movements = movementIt->second;
    const size_t size = movements.size();
    if (size == 0) {
        return std::nullopt; // no data
    }

    float work = 0;
    for (size_t i = 0; i < size - 1; i++) {
        const Movement& mvt = movements[i];
        const Movement& nextMvt = movements[i + 1];

        float vprev = kineticEnergyToVelocity(work);
        float delta = mDeltaValues ? nextMvt.position : nextMvt.position - mvt.position;
        float vcurr = delta / (SECONDS_PER_NANO * (nextMvt.eventTime - mvt.eventTime));
        work += (vcurr - vprev) * fabsf(vcurr);

        if (i == 0) {
            work *= 0.5; // initial condition, case 2) above
        }
    }

    const float velocity = kineticEnergyToVelocity(work);
    ALOGD_IF(DEBUG_STRATEGY, "velocity: %.1f", velocity);

    if (DEBUG_IMPULSE) {
        // TODO(b/134179997): delete this block once the switch to 'impulse' is complete.
        // Calculate the lsq2 velocity for the same inputs to allow runtime comparisons.
        // X axis chosen arbitrarily for velocity comparisons.
        VelocityTracker lsq2(VelocityTracker::Strategy::LSQ2);
        for (size_t i = 0; i < size; i++) {
            const Movement& mvt = movements[i];
            lsq2.addMovement(mvt.eventTime, pointerId, AMOTION_EVENT_AXIS_X, mvt.position);
        }
        std::optional<float> v = lsq2.getVelocity(AMOTION_EVENT_AXIS_X, pointerId);
        if (v) {
            ALOGD("lsq2 velocity: %.1f", *v);
        } else {
            ALOGD("lsq2 velocity: could not compute velocity");
        }
    }
    return velocity;
}

} // namespace android