/* @license
 * Copyright 2020 Google LLC. All Rights Reserved.
 * 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.
 */

export const SETTLING_TIME = 10000 // plenty long enough
const MIN_DECAY_MILLISECONDS = 0.001
export const DECAY_MILLISECONDS = 50

/**
 * The Damper class is a generic second-order critically damped system that does
 * one linear step of the desired length of time. The only parameter is
 * DECAY_MILLISECONDS. This common parameter makes all states converge at the
 * same rate regardless of scale. xNormalization is a number to provide the
 * rough scale of x, such that NIL_SPEED clamping also happens at roughly the
 * same convergence for all states.
 */
export class Damper {
  private velocity: number = 0
  private naturalFrequency: number = 0

  constructor(decayMilliseconds: number = DECAY_MILLISECONDS) {
    this.setDecayTime(decayMilliseconds)
  }

  setDecayTime(decayMilliseconds: number) {
    this.naturalFrequency = 1 / Math.max(MIN_DECAY_MILLISECONDS, decayMilliseconds)
  }

  update(
    x: number,
    xGoal: number,
    timeStepMilliseconds: number,
    xNormalization: number
  ): number {
    const nilSpeed = 0.0002 * this.naturalFrequency

    if (x === null || xNormalization === 0) {
      return xGoal
    }
    if (x === xGoal && this.velocity === 0) {
      return xGoal
    }
    if (timeStepMilliseconds < 0) {
      return x
    }
    // Exact solution to a critically damped second-order system, where:
    // acceleration = this.naturalFrequency * this.naturalFrequency * (xGoal
    // - x) - 2 * this.naturalFrequency * this.velocity;
    const deltaX = x - xGoal
    const intermediateVelocity = this.velocity + this.naturalFrequency * deltaX
    const intermediateX = deltaX + timeStepMilliseconds * intermediateVelocity
    const decay = Math.exp(-this.naturalFrequency * timeStepMilliseconds)
    const newVelocity =
      (intermediateVelocity - this.naturalFrequency * intermediateX) * decay
    const acceleration =
      -this.naturalFrequency * (newVelocity + intermediateVelocity * decay)
    if (
      Math.abs(newVelocity) < nilSpeed * Math.abs(xNormalization) &&
      acceleration * deltaX >= 0
    ) {
      // This ensures the controls settle and stop calling this function instead
      // of asymptotically approaching their goal.
      this.velocity = 0
      return xGoal
    } else {
      this.velocity = newVelocity
      return xGoal + intermediateX * decay
    }
  }
}