/**
 * Utility functions to replicate NumPy behavior
 */
const clip = (val: number, min: number, max: number): number => Math.min(Math.max(val, min), max)

const wrapToPi = (angle: number): number => Math.atan2(Math.sin(angle), Math.cos(angle))

/**
 * Calculates egocentric polar coordinates relative to a target
 */
const egocentricPolar = (
  target: [number, number, number],
  current: [number, number, number],
  backward: boolean = false,
): [number, number, number] => {
  const [tx, ty, tyaw] = target
  const [x, y, yaw] = current

  const dx = tx - x
  const dy = ty - y

  const r = Math.hypot(dx, dy)

  // Inverting dy for atan2 to match Python logic
  let los = Math.atan2(-dy, dx)
  if (backward) los += Math.PI

  const phi = wrapToPi(tyaw + los)
  const delta = wrapToPi(yaw + los)

  return [r, phi, delta]
}

class SmoothControlLaw {
  // Controller gains and constraints
  public k_phi: number
  public k_delta: number
  public beta: number
  public lam: number
  public slowdown_radius: number
  public v_linear_min: number
  public v_linear_max: number
  public v_angular_max: number

  constructor(
    k_phi: number = 3.0,
    k_delta: number = 3.0,
    beta: number = 0.3,
    lam: number = 1.0,
    slowdown_radius: number = 0.1,
    v_linear_min: number = 0.1,
    v_linear_max: number = 0.2,
    v_angular_max: number = 2.0,
  ) {
    this.k_phi = k_phi
    this.k_delta = k_delta
    this.beta = beta
    this.lam = lam
    this.slowdown_radius = slowdown_radius
    this.v_linear_min = v_linear_min
    this.v_linear_max = v_linear_max
    this.v_angular_max = v_angular_max
  }

  public curvature(r: number, phi: number, delta: number): number {
    if (r < 1e-6) return 0.0

    const prop = this.k_delta * (delta - Math.atan(-this.k_phi * phi))
    const feedback = (1 + this.k_phi / (1 + Math.pow(this.k_phi * phi, 2))) * Math.sin(delta)

    return (-1.0 / r) * (prop + feedback)
  }

  public velocity(
    target: [number, number, number],
    current: [number, number, number],
    backward: boolean = false,
  ): { v: number; w: number; kappa: number } {
    const [r, phi, delta] = egocentricPolar(target, current, backward)

    let kappa = this.curvature(r, phi, delta)
    if (backward) kappa = -kappa

    // Calculate linear velocity based on curvature
    let v = this.v_linear_max / (1 + this.beta * Math.pow(Math.abs(kappa), this.lam))

    // Slow down as we approach target
    v = Math.min(this.v_linear_max * (r / this.slowdown_radius), v)
    v = clip(v, this.v_linear_min, this.v_linear_max)

    if (backward) v = -v

    // Calculate angular velocity
    let w = kappa * v
    w = clip(w, -this.v_angular_max, this.v_angular_max)

    // Adjust v if w was clipped to maintain the path curvature
    if (Math.abs(kappa) > 1e-6) {
      v = w / kappa
    }

    return { v, w, kappa }
  }

  public step(
    target: [number, number, number],
    current: [number, number, number],
    dt: number,
    backward: boolean = false,
  ): [[number, number, number], number, number] {
    const { v, w } = this.velocity(target, current, backward)

    let [x, y, yaw] = current

    x += v * dt * Math.cos(yaw)
    y += v * dt * Math.sin(yaw)
    yaw += w * dt

    return [[x, y, yaw], v, w]
  }
}

export { SmoothControlLaw }
