/**
 * CPU-side double pendulum physics mirroring PENDULUM_PHYSIC_COMPUTE_SHADER_WGSL.
 * Equations from: https://en.wikipedia.org/wiki/Double_pendulum
 */

export interface PendulumState {
  theta1: number;
  theta2: number;
  v1: number;
  v2: number;
}

export interface PendulumPhysicsParams {
  dt: number;
  g: number;
  m1: number;
  m2: number;
  l1: number;
  l2: number;
  damping: number;
  impulseM1: number;
  impulseM2: number;
}

export function stepPendulumPhysics(state: PendulumState, params: PendulumPhysicsParams): void {
  const t1 = state.theta1;
  const t2 = state.theta2;
  const v1 = state.v1;
  const v2 = state.v2;

  const deltaT = t1 - t2;

  const num1 = -params.g * (2.0 * params.m1 + params.m2) * Math.sin(t1)
    - params.m2 * params.g * Math.sin(t1 - 2.0 * t2)
    - 2.0 * Math.sin(deltaT) * params.m2 * (v2 * v2 * params.l2 + v1 * v1 * params.l1 * Math.cos(deltaT));
  const den1 = params.l1 * (2.0 * params.m1 + params.m2 - params.m2 * Math.cos(2.0 * deltaT));
  const a1 = num1 / den1;

  const num2 = 2.0 * Math.sin(deltaT) * (v1 * v1 * params.l1 * (params.m1 + params.m2) + params.g * (params.m1 + params.m2) * Math.cos(t1) + v2 * v2 * params.l2 * params.m2 * Math.cos(deltaT));
  const den2 = params.l2 * (2.0 * params.m1 + params.m2 - params.m2 * Math.cos(2.0 * deltaT));
  const a2 = num2 / den2;

  const newV1 = (v1 + a1 * params.dt) * params.damping + params.impulseM1;
  const newV2 = (v2 + a2 * params.dt) * params.damping + params.impulseM2;

  state.v1 = newV1;
  state.v2 = newV2;
  state.theta1 = t1 + newV1 * params.dt;
  state.theta2 = t2 + newV2 * params.dt;
}
