# ======================================================================== #
#
# Copyright 2012, Gabor Hegedus, Josef Schicho, and Hans-Peter Schroecker
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
#
# ======================================================================== #

# Maple implementation of the factorization algorithm for rational
# parametrizations of cubic curves on the Study quadric.
#
# Dual quaternions are stored as quadruples whose entries are of the
# shape a + b * e where e is the dual unit.
#
# Monic polynomials
#
#     t^n + a_{n-1}*t^{n-1} + ... + a_1*t + a_0
#
# with dual quaternion coefficients a_{n-1}, ..., a_1, a_0 are stored
# as list
#
#     [a_{n-1}, ..., a_1, a_0]
#
# with dual quaternion entries.

# ==================================================
# Procedures
# ==================================================

e := epsilon:    # shorthand notation

Cj := proc(a)
  # conjugate dual quaternion
  [ a[1],-a[2],-a[3],-a[4] ];
end proc;

Mul := proc(a,b)
  # product of dual quaternions
  local raw,T;
  raw:=
  [ a[1]*b[1]-a[2]*b[2]-a[3]*b[3]-a[4]*b[4],
    a[1]*b[2]+a[2]*b[1]+a[3]*b[4]-a[4]*b[3],
    a[1]*b[3]-a[2]*b[4]+a[3]*b[1]+a[4]*b[2],
    a[1]*b[4]+a[2]*b[3]-a[3]*b[2]+a[4]*b[1]
  ];
  map(T->rem(expand(T),e^2,e),raw);
end proc;

Mulall := proc()
  # product of all dual quaternions in a list
  local res,i;
  res := [1,0,0,0];
  for i from 1 to nargs do
    res := Mul(res,args[i]);
  od;
  res;
end proc;

PolMult := proc(P,Q)
  # Product of two monic left polynomials. A left polynomial is
  # represented by the coefficient list starting with coeff. of
  # t^{d-1}.
  local m,n,ret,i,k,cf;
  m := nops(P);
  n := nops(Q);
  ret := [];
  for i from 1 to m+n do
    cf := [0,0,0,0];
    if i <= m then cf := cf+P[i] fi;
    if i <= n then cf := cf+Q[i] fi;
    if i > 1 then
    for k from max(1,i-m) to min(i-1,n) do
      cf := cf + Mul(P[i-k],Q[k]);
    od;
    fi;
  cf := map(expand,cf);
  ret := [op(ret),cf];
  od;
  ret;
end proc;

MinPol := proc(h);
  # Compute the minimal polynomial of a dual quaternion.
  [ -h-Cj(h), Mul(h,Cj(h))]
end proc;

PolQuo := proc(P,B)
  # Quotient of P right divided by B, where P is arbitrary and B is
  # monic.
  local out,PP,i,lc;
  if nops(P)<=nops(B) then
    out := [];
  else
    lc := P[1];
    PP := [];
    for i from 2 to nops(B)+1 do
      PP := [op(PP),P[i]-Mul(lc,B[i-1])];
    od;
    for i from nops(B)+2 to nops(P) do
      PP := [op(PP),P[i]];
    od;
    out := [lc,op(PolQuo(PP,B))];
  fi;
  out;
end proc;

PolRem := proc(P,B)
  # Remainder of P right divided by B, where B is monic and P is
  # arbitrary. Note that P is a list of length deg P, B is a list of
  # length (deg B - 1).
  local out,PP,i,lc;
  if nops(P)<=nops(B) then
    out := P;
  else
    lc := P[1];
    PP := [];
    for i from 2 to nops(B)+1 do
      PP := [op(PP),P[i]-Mul(lc,B[i-1])];
    od;
    for i from nops(B)+2 to nops(P) do
      PP := [op(PP),P[i]];
    od;
    out := PolRem(PP,B);
  fi;
  out;
end proc;

Smul := proc(k,a)
  # Product of dual scalar with dual quaternion.
  local raw,T;
  raw :=
  [ k*a[1],k*a[2],k*a[3],k*a[4] ];
  map(T->rem(expand(T),e^2,e),raw);
end proc;

Sinv := proc(k)
  # Inverse of dual scalar.
  local kp,kd;
  kp := coeff(k,e,0);
  kd := coeff(k,e,1);
  1/kp-e*kd/kp^2;
end proc;

Quotl := proc(a,b)
  # Left quotient a^{-1}b.
  local ab,ac;
  ac := Cj(a);
  ab := Mul(a,ac)[1];
  Smul(Sinv(ab),Mul(ac,b));
end proc;

Solution := proc(P,M)
  # Compute the unique solution of P with minimal polynomial M.
  local Lin;
  Lin := PolRem(AddLead(P),M);
  Quotl(-Lin[1],Lin[2]);
end proc;

AddLead := proc(P)
  # Add leading coefficient formally to a monic polynomial.
  [[1,0,0,0],op(P)];
end proc;

RemoveLead := proc(P);
  # Remove leading coefficient of monic polynomial.
  [seq(P[i],i=2..nops(P))];
end proc;

PolQuot := proc(P,B)
  # Quotient of P right divided by B, where P and B are monic and the
  # output is also monic
  RemoveLead(PolQuo(AddLead(P),B));
end proc;

shift := proc(L,i)
  # cyclic shift of list 
  local res;
  res := op(i..nops(L),L);
  res := res,op(1..i-1,L);
  [res];
end proc;

ldist := proc(l1,l2)
  # Orthogonal distance of two lines.
  local l1o,l2o,prd,pra,res;
  l1o := l1-(l1+Cj(l1))/2;
  l2o := l2-(l2+Cj(l2))/2;
  pra := Mul(l1o,Cj(l1o))[1]*Mul(l2o,Cj(l2o))[1];
  if lang(l1,l2) <> 0 then
    prd := Mul(l1o,l2o)[1];
    res := normal(coeff(prd,e,1)^2/
                 (pra-coeff(prd,e,0)^2));
  else
    prd := subs(epsilon=1,Mul(l1o,l2o)-Mul(l2o,l1o));
    res := normal(Mul(prd,Cj(prd))[1]/pra/4);
  fi;
  sqrt(res,symbolic);
end proc;

ldists := proc(L)
  # All orthogonal distances in a linkage.
  local i,res,n;
  n := nops(L);
  res := ldist(L[n],L[1]);
  for i from 1 to n-1 do
    res := res, ldist(L[i],L[i+1])
  od;
  [res];
end proc;

offset := proc(l1,l2,l3)
  # Compute offset of the three lines l1, l2, l3.
  local l1o,l2o,l3o,cor,l12,l23;
  l1o := l1-(l1+Cj(l1))/2;
  l2o := l2-(l2+Cj(l2))/2;
  l3o := l3-(l3+Cj(l3))/2;
  l12 := Mul(l1o,l2o)-Mul(l2o,l1o);
  cor := subs(epsilon=-epsilon/2,Mul(l12,Cj(l12)))[1];
  l12 := Smul(cor,l12);
  l23 := Mul(l2o,l3o)-Mul(l3o,l2o);
  cor := subs(epsilon=-epsilon/2,Mul(l23,Cj(l23)))[1];
  l23 := Smul(cor,l23);
  ldist(l12,l23);
end proc;

offsets := proc(L)
  # All orthogonal distances in a linkage.
  local i,res,n;
  n := nops(L);
  res := offset(L[n-1],L[n],L[1]);
  res := res,offset(L[n],L[1],L[2]);
  for i from 1 to n-2 do
    res := res, offset(L[i],L[i+1],L[i+2])
  od;
  shift([res],2);
end proc;

lang := proc(l1,l2)
  # Compute angle of two lines.
  local l1o,l2o,prd,pra,res;
  l1o := l1-(l1+Cj(l1))/2;
  l2o := l2-(l2+Cj(l2))/2;
  prd := Mul(l1o,l2o)[1];
  pra := Mul(l1o,Cj(l1o))[1]*Mul(l2o,Cj(l2o))[1];
  normal(coeff(prd,e,0)^2/pra);
  res:=rem(coeff(prd,e,0)^2*Sinv(pra),e^2,e);
  res:=arccos(sqrt(res));
end proc;

langs := proc(L)
  # All angles in a linkage.
  local i,res,n;
  n := nops(L);
  res := lang(L[n],L[1]);
  for i from 1 to n-1 do
    res := res, lang(L[i],L[i+1])
  od;
  [res];
end proc;

PlugIn := proc(P, h)
  # evaluation of polynomial P at dual quaternion h
  local res,i;
  res := [1,0,0,0];
  for i from 1 to nops(P) do
    res := Mul(res,h)+P[i];
  od;
  res;
end proc;