triginverses: all$
assume(theta >= 0)$
assume(theta <= %pi)$
assume(sin(theta) >= 0)$

LP(l, x) := legendre_p(l, x)$

ALP(l, m, x) := assoc_legendre_p(l, m, x)$

ALP_cos(l, m, theta) := (-1)^m*(sin(theta))^abs(m)*subst(cos(theta), x, diff(LP(l, x), x, abs(m)))$

K(l, m) := sqrt((2*l+1)/(4*%pi)*((l-abs(m))!/(l+abs(m))!))$

SH(l, m, theta, phi) := 
  if m > 0 then
    sqrt(2)*K(l, m)*cos(m*phi)*ALP_cos(l, m, theta)
  else 
    if m < 0 then
      sqrt(2)*K(l, m)*sin(-m*phi)*ALP_cos(l, -m, theta)
    else
      K(l, 0)*ALP_cos(l, 0, theta)$

int_on_unit_sphere(f) := block(
  [int_theta, int_phi],
  assume(notequal(cos(theta), 0)),
  f: trigsimp(trigexpand(f)),
  int_phi: integrate(f, phi, 0, 2*%pi),
  int_theta: integrate(int_phi*sin(theta), theta, 0, %pi)
)$

/* M_{ij} = \int_{S} Y_i(\mathbf{R}s)Y_j(s) ds, see [Robin Green 03] */

/* calculate $\hat R_Z^{\alpha}$ */
rot_sh_z(alpha, l) := block(
  [rtn],
  rtn: zeromatrix(2*l+1, 2*l+1),
  for i:-l thru l do
    for j:-l thru l do
      rtn[i+l+1, j+l+1]: trigsimp(int_on_unit_sphere(SH(l, i, theta, phi+alpha)*SH(l, j, theta, phi))),
  rtn
)$

rot_sphere_coord_x(alpha, theta, phi) := block(
  [X, Y, R],
  X: matrix([sin(theta)*cos(phi)], [sin(theta)*sin(phi)], [cos(theta)]),
  R: zeromatrix(3, 3),
  R[1, 1]: 1,
  R[2, 2]: cos(alpha),
  R[3, 3]: cos(alpha),
  R[2, 3]: -sin(alpha),
  R[3, 2]: sin(alpha),
  /* display(R), */
  Y: R.X,
  matrix([acos(Y[3, 1])], [atan2(Y[2, 1], Y[1, 1])])
)$

/* calculate $\hat R_X^{\alpha}$ */
rot_sh_x(alpha, l) := block(
  [rot_sphere_coord, rtn],
  rtn: zeromatrix(2*l+1, 2*l+1),
  rot_sphere_coord: rot_sphere_coord_x(alpha, theta, phi),
  for i:-l thru l do
    for j:-l thru l do block(
      temp: trigexpand(trigsimp(SH(l, i, rot_sphere_coord[1, 1], rot_sphere_coord[2, 1]))), /*!*/
      temp: temp*SH(l, j, theta, phi),
      temp: subst([(cos(phi)^2*sin(theta)^2+cos(theta)^2)=1-sin(phi)^2*sin(theta)^2], temp),
      temp: trigexpand(temp),
      /* display(temp), */
      rtn[i+l+1, j+l+1]: radcan(int_on_unit_sphere(temp))
    ),
  rtn
)$

RZ4_ALPHA:       rot_sh_z(alpha, 4)$
RX2_POS_HALF_PI: rot_sh_x(%pi/2, 2)$
RX4_POS_HALF_PI: rot_sh_x(%pi/2, 4)$
RX4_NEG_HALF_PI: transpose(RX4_POS_HALF_PI)$
CUB_SYM_H_COEF: transpose(matrix([0, 0, 0, 0, sqrt(7), 0, 0, 0, sqrt(5)]))$

/* CONVERT ZYZ ANGLES TO SH COEFFS */
zyz2sh(zyz) := rot_sh_z(zyz[3, 1], 4).RX4_NEG_HALF_PI.rot_sh_z(zyz[2, 1], 4).RX4_POS_HALF_PI.rot_sh_z(zyz[1, 1], 4).CUB_SYM_H_COEF$

NORM(X) := sqrt(X.X)$

/*
 * energy term for smoothness
 * ABC [3x2]: zyz angle of two adjacent frames
 * stiff [1x1]: intergral weight, see equation 2 in paper
 * http://www.cad.zju.edu.cn/home/hj/16/closed-form-polycube/closed-form-polycube.pdf
 */
cubic_sym_smooth_tet(ABC, stiff) := block(
  [c1, c2],
  c1: zyz2sh(col(ABC, 1)),
  c2: zyz2sh(col(ABC, 2)),
  stiff*NORM(c1-c2)^2
)$
abc: genmatrix(abc, 3, 2)$
w: genmatrix(w, 1, 1)$
smooth_val: cubic_sym_smooth_tet(abc, w);

/*
 * energy term for normal alignment
 * ABC [3x1]: zyz angle of a frame
 * Rnz [3x1]: zyz angle to align face normal and z axis
 * area [1x1]: face area
 */
cubic_sym_align(ABC, Rnz, area) := block(
  [coef],
  coef: zyz2sh(ABC),
  coef: rot_sh_z(Rnz[3, 1], 4).RX4_NEG_HALF_PI.rot_sh_z(Rnz[2, 1], 4).RX4_POS_HALF_PI.rot_sh_z(Rnz[1, 1], 4).coef,
  area*(coef[5, 1]-sqrt(7))^2
)$
def: genmatrix(def, 3, 1)$
Rnz: genmatrix(Rnz, 3, 1)$
area: genmatrix(area, 1, 1)$
align_val: cubic_sym_align(def, Rnz, area);
