// Dihedral angle across a hinge and its gradient.
//
// Two triangles (i, j, k) and (l, k, j) share the edge j-k; i and l are the
// opposite vertices. theta is the signed angle between the two triangle
// normals about the edge (0 when the triangles are coplanar). The gradient
// is that of Bridson, Marino and Fedkiw (2003), "Simulation of clothing
// with folds and wrinkles".

// Writes d theta / d(x_i, x_j, x_k, x_l) into g[off .. off+12) and returns theta.
export function hingeAngle(x: Float64Array, i: number, j: number, k: number, l: number, g: Float64Array, off = 0): number {
  const x1 = 3 * i; // opposite vertex of the first triangle
  const x2 = 3 * l; // opposite vertex of the second triangle
  const x3 = 3 * j; // edge start
  const x4 = 3 * k; // edge end
  const e0 = x[x4] - x[x3];
  const e1 = x[x4 + 1] - x[x3 + 1];
  const e2 = x[x4 + 2] - x[x3 + 2];
  const eLen = Math.sqrt(e0 * e0 + e1 * e1 + e2 * e2);
  // n1 = (x1 - x3) x (x1 - x4), n2 = (x2 - x4) x (x2 - x3)
  const a0 = x[x1] - x[x3], a1 = x[x1 + 1] - x[x3 + 1], a2 = x[x1 + 2] - x[x3 + 2];
  const b0 = x[x1] - x[x4], b1 = x[x1 + 1] - x[x4 + 1], b2 = x[x1 + 2] - x[x4 + 2];
  const c0 = x[x2] - x[x4], c1 = x[x2 + 1] - x[x4 + 1], c2 = x[x2 + 2] - x[x4 + 2];
  const d0 = x[x2] - x[x3], d1 = x[x2 + 1] - x[x3 + 1], d2 = x[x2 + 2] - x[x3 + 2];
  const n10 = a1 * b2 - a2 * b1, n11 = a2 * b0 - a0 * b2, n12 = a0 * b1 - a1 * b0;
  const n20 = c1 * d2 - c2 * d1, n21 = c2 * d0 - c0 * d2, n22 = c0 * d1 - c1 * d0;
  const n1sq = n10 * n10 + n11 * n11 + n12 * n12;
  const n2sq = n20 * n20 + n21 * n21 + n22 * n22;
  // angle from atan2((n1 x n2) . e/|e|, n1 . n2)
  const cr0 = n11 * n22 - n12 * n21, cr1 = n12 * n20 - n10 * n22, cr2 = n10 * n21 - n11 * n20;
  const sinPart = (cr0 * e0 + cr1 * e1 + cr2 * e2) / eLen;
  const cosPart = n10 * n20 + n11 * n21 + n12 * n22;
  // sign chosen to match the gradient below
  const theta = -Math.atan2(sinPart, cosPart);
  // gradient
  const s1 = eLen / n1sq;
  const s2 = eLen / n2sq;
  // u1 = |e| n1/|n1|^2, u2 = |e| n2/|n2|^2
  const p1 = (a0 * e0 + a1 * e1 + a2 * e2) / eLen; // (x1 - x3).e/|e|
  const q1 = (b0 * e0 + b1 * e1 + b2 * e2) / eLen; // (x1 - x4).e/|e|
  const p2 = (d0 * e0 + d1 * e1 + d2 * e2) / eLen; // (x2 - x3).e/|e|
  const q2 = (c0 * e0 + c1 * e1 + c2 * e2) / eLen; // (x2 - x4).e/|e|
  const m1 = 1 / n1sq;
  const m2 = 1 / n2sq;
  const aj = q1 * m1, bj = q2 * m2; // x_j (edge start)
  const ak = -p1 * m1, bk = -p2 * m2; // x_k (edge end)
  g[off] = s1 * n10; // x_i
  g[off + 1] = s1 * n11;
  g[off + 2] = s1 * n12;
  g[off + 3] = aj * n10 + bj * n20;
  g[off + 4] = aj * n11 + bj * n21;
  g[off + 5] = aj * n12 + bj * n22;
  g[off + 6] = ak * n10 + bk * n20;
  g[off + 7] = ak * n11 + bk * n21;
  g[off + 8] = ak * n12 + bk * n22;
  g[off + 9] = s2 * n20; // x_l
  g[off + 10] = s2 * n21;
  g[off + 11] = s2 * n22;
  return theta;
}

// theta - theta0 wrapped to (-pi, pi]
export function angleDiff(theta: number, theta0: number): number {
  let d = theta - theta0;
  if (d > Math.PI) d -= 2 * Math.PI;
  else if (d <= -Math.PI) d += 2 * Math.PI;
  return d;
}