(*  PlanarLinkages -- construction of planar linkages following a prescribed motion
 *  Copyright (C) 2015  Christoph Koutschan
 *
 *  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, see http://www.gnu.org/licenses/
 *
 *  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.
 *)

If[$Notebooks,
  CellPrint[Cell[TextData[{#1, StyleBox[#2, Small]}], "Text", FontColor -> Black, CellFrame -> 0.5,
    Background -> RGBColor[0.796887, 0.789075, 0.871107]]],
  Print[#1 <> #2]]&[
  "PlanarLinkages   \[LongDash]   Copyright \[Copyright] 2015   Christoph Koutschan\n",
  "This program comes with absolutely no warranty; it is free software,\n" <>
  "and you are welcome to redistribute it and/or modify it under the\n" <>
  "terms of the GNU General Public License (http://www.gnu.org/licenses/)."];

(* Introduce a data type MP that represents (possibly constant) 2D motion polynomials:
   MP[x1, x2, x3, x4] encodes (x1 + I*x2) + eta*(x3 + I*x4). *)
(* The symbol eta is automatically transformed into a motion polynomial. *)
eta := MP[0, 1];

(* Define UpValues for MP. We want that addition and multiplication of MP objects are
 * automatically performed, unless the input is ambiguous concerning the order of factors:
 * if MP1*MP2 is input then Mathematica may reorder the factors, use MP1**MP2 instead.
 * We assume that in any input eta is on the left, i.e., I*eta is interpreted as eta*I. *)
MP /: Plus[a_MP, b__] := AddMP[a, b];
MP /: Times[a_MP, b__ /; FreeQ[{b}, MP]] := MultMP[a, b];
MP /: NonCommutativeMultiply[b1___, a_MP, b2___] := MultMP[b1, a, b2];

(* How to display motion polynomials. The \[Eta] is always visible in order to enable the user
 * to distinguish MP objects from other Mathematica expressions, e.g., MP[1, 0] from 1. *)
Format[MP[z_, w1_]] :=
Module[{w = w1, op},
  op = If[MatchQ[MakeBoxes @@ {w}, RowBox[{"-", _}]], w = -w; "-", "+"];
  Infix[{z, If[w === 1, \[Eta], CenterDot[\[Eta], w]]}, op]];

(* Introduce a data type FactoredMP that is used to keep a factorization of a motion polynomial. *)
Format[FactoredMP[ms: (_MP)..]] := CenterDot[ms];
FactoredMP /: Expand[FactoredMP[ms: (_MP)..]] := MultMP[ms];

(* Interpret copy-and-paste input correctly. *)
MakeExpression[InterpretationBox[RowBox[{a1_, op : ("+" | "-"), a2:("\[Eta]" | RowBox[{"\[Eta]", "\[CenterDot]", _}])}],
  _, Editable -> False], StandardForm] :=
  With[{sp = If[a2 === "\[Eta]", 1, a2[[1,3]]]},
    MakeExpression[RowBox[{"MP", "[", RowBox[{a1, ",", If[op === "+", sp, RowBox[{"-", sp}]]}], "]"}], StandardForm]];
MakeExpression[RowBox[{a1__, op : ("+" | "-"), a2:("\[Eta]" | RowBox[{"\[Eta]", "\[CenterDot]", _}])}], StandardForm] :=
  With[{sp = If[a2 === "\[Eta]", 1, a2[[1,3]]]},
    MakeExpression[RowBox[{"MP", "[", RowBox[{RowBox[{a1}], ",", If[op === "+", sp, RowBox[{"-", sp}]]}], "]"}], StandardForm]];
MakeExpression[RowBox[list_] /; Union[Take[list, {2, -1, 2}]] === {"\[CenterDot]"}, StandardForm] :=
  MakeExpression[RowBox[{"FactoredMP", "[", RowBox[
      Riffle[MakeBoxes[ReleaseHold[MakeExpression[#, StandardForm]]] & /@ Take[list, {1, -1, 2}], ","]
    ], "]"}], StandardForm];

(* Convert an (eta-free) expression to a motion polynomial + do some consistency checks. *)
ToMP[a_] :=
  With[{args = If[Head[a] === MP, List @@ a, {a, 0}]},
    If[Length[args] === 2 && FreeQ[args, MP], MP @@ args, Throw["Cannot convert to motion polynomial."]]];

(* Simplify a motion polynomial. *)
SimpMP[p_MP] := SimpMP[p, t];
SimpMP[MP[z_, w_], t_] := Collect[#, t]& /@ (MP[z, w] /. {Conjugate[t] -> t, Re[t] -> t, Im[t] -> 0});

(* Addition and Multiplication of motion polynomials. *)
AddMP[] := MP[0, 0];
AddMP[ms: (_MP)..] := SimpMP[MP @@ Total[List @@@ {ms}]];
AddMP[ms__] := AddMP @@ (ToMP /@ {ms});
MultMP[] := MP[1, 0];
MultMP[MP[z1_, w1_], MP[z2_, w2_]] := SimpMP[MP @@ (Expand /@ {z1 * z2, Conjugate[z1] * w2 + w1 * z2})];
MultMP[ms: (_MP)..] := Fold[MultMP, First[{ms}], Rest[{ms}]];
MultMP[ms__] := MultMP @@ (ToMP /@ {ms});

(* The 3*3 matrix associated to a motion polynomial; it encodes the action
 * of that polynomial on P^3. *)
MP2Mat[MP[z_, w_]] :=
Module[{e0, e12, e1, e2},
  {e0, e12, e1, e2} = (# . t ^ (Range[Length[#]] - 1))& /@
    Flatten[{Re[#], Im[#]}& /@ CoefficientList[{z, w}, t], 1];
  {{ e0^2 + e12^2  ,      0       ,      0       },
   {e0*e1 - e2*e12 , e0^2 - e12^2 ,  -2*e0*e12   },
   {e0*e2 + e1*e12 ,   2*e0*e12   , e0^2 - e12^2 }}];

(* Action of a motion polynomial on points in R^2 *)
ActR2::usage = "ActR2[p, {x, y}] computes the result of applying the direct isometry given by p " <>
"to the point {x, y}, where p is a (possibly constant) motion polynomial.";
ActR2[p_MP, {x_, y_}] := With[{v = MP2Mat[p] . {1, x, y}}, Together[Rest[v] / First[v]]];

(* Given complex polynomials Z and W (by their lists of roots), find *)
(* a permutation of the roots of R*Z (for some real polynomial R), *)
(* such that R * (Z + eta * W) can be factored into linear factors. *)
FindOrder[rtsZ_List, rtsW_List] :=
Module[{rts = rtsZ, alpha, albar, r, s, u, v, m, q = {}},

  While[rts =!= {},
    alpha = First[rts];
    albar = ComplexExpand[Conjugate[alpha]];
    (* Make always r_i >= s_i. *)
    If[Count[rts, alpha] <= Count[rts, albar], {alpha, albar} = {albar, alpha}];
    {r, s} = Count[rts, #]& /@ {alpha, albar};
    rts = DeleteCases[rts, alpha | albar];
    {u, v} = Count[rtsW, #]& /@ {alpha, albar};
    If[s * u * v > 0, Throw["Z and W have common real factors."]];
    m = Min[s, u + v];
    q = Join[q, Table[albar, {s - Min[s, v]}], Table[alpha, {r + s - m}], Table[albar, {s - Min[s, u]}]]
  ];

  Return[q];
];

(* Factorization of motion polynomials *)
FactorMP::usage = "FactorMP[P] factors the given normed bounded motion polynomial P into linear factors. " <>
  "If necessary, P is multiplied by a real polynomial R and the factorization of R*P is computed; this " <>
  "situation is indicated by a message. With the option Order -> {r1, r2, ...} one can specify an order " <>
  "in which the roots of Z, the primal part of P, should appear in the factorization. Note that then R " <>
  "is not automatically determined; however, one can do so by giving more roots than just those of Z.";
FactorMP::R = "Multiply the input with R = `1`";
Options[FactorMP] = {Order -> Automatic};
FactorMP[P_MP, opts:(_Rule|_RuleDelayed)...] := FactorMP[P, t, opts];
(* Notation is the same as in the paper. *)
FactorMP[MP[Z_, W_], t:Except[_Rule|_RuleDelayed], opts:(_Rule|_RuleDelayed)...] :=
Module[{error, rtsZ, rtsW, R, q, n, Qs, mat, ns, v, ws},
  q = Order /. {opts} /. Options[FactorMP];
  error = "FactorMP: input must be a bounded and normed motion polynomial.";
  If[Z === 0, Throw[error]];
  {rtsZ, rtsW} = Function[poly, ComplexExpand[#[[1, 2]]]& /@ Solve[poly == 0, t]] /@ {Z, W};
  (* Check normedness, i.e., deg(W) < deg(Z) and lc(Z) = 1. *)
  If[Length[rtsW] >= Length[rtsZ] || Coefficient[Z, t^Length[rtsZ]] =!= 1, Throw[error]];
  (* Check boundedness, i.e., no real root. *)
  If[Or @@ (Element[#, Reals]& /@ rtsZ), Throw[error]];

  q = If[q === Automatic, FindOrder[rtsZ, rtsW], ComplexExpand[q]];
  R = ComplexExpand[Expand[Together[(Times @@ (t - # & /@ q)) / (Times @@ (t - # & /@ rtsZ))]]];
  If[Not[PolynomialQ[R, t]], Throw["The given zeros must contain all zeros of Z (with multiplicity!)."]];
  If[R =!= 1, Message[FactorMP::R, R]];

  (* From this ansatz build a linear system and solve it. *)
  n = Length[q];
  Qs = Table[Product[t - Conjugate[q[[l]]], {l, j - 1}] * Product[t - q[[l]], {l, j + 1, n}], {j, n}];
  mat = Transpose[PadRight[CoefficientList[#, t]& /@ Append[Qs, R * W]]];
  ns = NullSpace[mat];

  (* The following should only happen when the user gives bad options. *)
  If[Select[ns, Last[#] =!= 0 &] === {}, Throw["No solution found."]];

  (* Construct a parametric solution of the inhomogeneous system. *)
  v = First[SortBy[Select[ns, Last[#] =!= 0 &], ByteCount[Last[#]]&]];
  v = Together[v / Last[v]];
  ns = DeleteCases[Together[# - Last[#] * v]& /@ ns, {(0)..}];
  ws = ComplexExpand[-Most[Total[ns * Array[C, Length[ns]]] + v]];

  Return[FactoredMP @@ (MP[t - #1, #2]& @@@ Transpose[{q, ws}])];
];

(* FixPoint of a revolution *)
FixPoint::usage = "FixPoint[p] computes the fixpoint of a simple revolution given by " <>
"the linear normed motion polynomial p, i.e., p is of the form t - z + \[Eta]*w where " <>
"z and w are complex numbers with z being non-real.";
FixPoint[m_MP] := FixPoint[m, t];
FixPoint[MP[z_, w_], t_] :=
Module[{},
  If[Not[FreeQ[{z - t, w}, t]] || Im[z - t] === 0,
    Throw["FixPoint: Input is not a normed bounded motion polynomial of degree 1."]];
  Return[{-Im[w], Re[w]} / (2 Im[z - t])];
];

(* Compute the lengths of the links associated to a factored motion polynomial. *)
Options[LinkLengths] = {Trace -> {0, 0}};
LinkLengths[FactoredMP[mps__], opts:((_Rule|_RuleDelayed)...)] :=
  PowerExpand[Simplify[
    Sqrt[#1^2 + #2^2]& @@@ Differences[Append[FixPoint /@ Reverse[{mps}], Trace /. {opts} /. Options[LinkLengths]]]
  ]];

(* Create Graphics primitives for trace (called by AnimateMP and ShowLinkage). *)
TraceMP[m_MP, t_, p_, plotrange_, range_ : {-Pi/2 + 0.01, Pi/2 - 0.01}, dir1_ : {0, 0}, d3_ : False] :=
Module[{dir = dir1, b, d, x, dx, ps, pt, pts, step, r1, r2},
  {r1, r2} = range;
  step = Max[10^(-6), Max[Differences /@ plotrange] / 100];
  dx = 0.1; x = r1;
  ps = Union[{p - dir, p + dir}];
  pts = {ActR2[m /. t -> Tan[x], #]& /@ ps};
  While[x < r2,
    pt = ActR2[m /. t -> Tan[Min[x + dx, r2]], #]& /@ ps;
    d = Max[Norm /@ (pt - pts[[-1]])];
    If[d < step || dx < 1/1000,
      AppendTo[pts, pt];
      If[d < step/2 && dx < 0.1, dx *= 2];
      x += dx;
    ,
      dx /= 2;
    ];
  ];
  If[Max[Norm /@ (pts[[1]] - pts[[-1]])] < step, AppendTo[pts, pts[[1]]]];
  If[d3 =!= False,
    pts = Map[Append[#, d3]&, pts, {2}];
    AppendTo[dir, 0];
  ];
  Return[If[dir === -dir, Line[First /@ pts], {EdgeForm[], Polygon[#[[{1,2,4,3}]]& /@ (Join @@@ Partition[pts, 2, 1])]}]];
];

(* Create Graphics primitives for MP and FactoredMP (called by AnimateMP). *)
Options[ToGraphics] = {Style -> Automatic, Polygon -> {{0, 0}, {1, 0}, {0, 1}}};
ToGraphics::zerolen := "Component of length zero encountered.";
ToGraphics[m_MP, opts:((_Rule|_RuleDelayed)...)] := ToGraphics[m, t, opts];
ToGraphics[m_MP, t:Except[_Rule|_RuleDelayed], opts:((_Rule|_RuleDelayed)...)] :=
Module[{z, pgon, trace, poly},
  If[m[[1]] === 0, Return[Function[tt, Polygon[{{0, 0}, {0, 1}}]]]];
  {pgon, trace} = {Polygon, Trace} /. {opts} /. Options[ToGraphics];
  If[Not[MatchQ[trace, {_, _}]], trace = {0, 0}];
  pgon = (# + trace)& /@ pgon;
  poly = m /. t -> z;
  Return[Function[tt, {Polygon[ActR2[poly /. z -> tt, #] & /@ pgon]}]];
];
ToGraphics[fact_FactoredMP, opts:((_Rule|_RuleDelayed)...)] := ToGraphics[fact, t, opts];
ToGraphics[fact_FactoredMP, t:Except[_Rule|_RuleDelayed], opts:((_Rule|_RuleDelayed)...)] :=
Module[{a1, a2, z, style, pgon, trace, fps, rs, pts, polys},
  {style, pgon, trace} = {Style, Polygon, Trace} /. {opts} /. Options[ToGraphics];
  If[style === Automatic, style = "links"];
  If[Not[MatchQ[trace, {_, _}]], trace = {0, 0}];
  fps = Prepend[FixPoint /@ (List @@ fact), trace];
  rs = Norm /@ Differences[fps];
  If[MemberQ[rs, 0], Message[ToGraphics::zerolen]];
  pts = Table[
    {a1, a2} = fps[[i - 1]] - fps[[i]];
    (fps[[i]] + #) & /@ {{0, 0}, {a1, a2}, {-a1, -a2}, {a2, -a1}, {-a2, a1}}, {i, 2, Length[fps]}];
  polys = Reverse[FoldList[MultMP[#2, #1] &, Last[fact], Rest[Reverse[List @@ fact]]]] /. t -> z;
  Return[Function[tt,
    Module[{ps, r = {}},
      Do[
        ps = ActR2[polys[[i]] /. z -> tt, #] & /@ pts[[i]];
        r = Join[r, Switch[style,
          "wheels", {Circle[ps[[1]], rs[[i]]], Line[{ps[[2]], ps[[3]]}], Line[{ps[[4]], ps[[5]]}]},
          "links", {Line[{ps[[1]], ps[[2]]}]}
        ]];
      , {i, Length[pts]}];
      Join[{Gray, Opacity[0.5], Polygon[ActR2[polys[[1]] /. z -> tt, #] & /@ pgon], Black, Opacity[1]}, r]]]];
];

(* Create animation of a motion. *)
AnimateMP::usage = "AnimateMP[p] animates the motion described by the motion polynomial p. If p is given " <>
"in factored form, then the decomposition of the motion into revolutions is shown. AnimateMP[p1, p2, ...] " <>
"shows several motions at the same time. The following options can be given:\n" <>
"Trace -> {x, y}: show the trace of the point {x, y} under the given motion.\n" <>
"Style -> \"links\", Style -> \"wheels\": different visualizations of a decomposed motion.\n" <>
"Polygon -> {{0, 0}, {1, 0}, {0, 1}}: specify the shape of a planar object that is displaced according to p.\n" <>
"Moreover, all options of the Graphics command can be used.";
Options[AnimateMP] = {Axes -> True, ImageSize -> {400, 300}, Trace -> {0, 0}}; (* + options in ToGraphics *)
AnimateMP[args:(_MP|_FactoredMP).., opts:(_Rule|_RuleDelayed)...] := AnimateMP[args, t, opts];
AnimateMP[args:(_MP|_FactoredMP).., t:Except[_MP|_FactoredMP|_Rule|_RuleDelayed], opts:(_Rule|_RuleDelayed)...] := 
Module[{x, fs, c, pts, range, tr, opts1, pi2 = Pi/2 - 0.01},
  fs = ToGraphics[#, opts]& /@ {args};
  c = Quiet[Cases[Flatten[Table[#[Tan[x]]& /@ fs, {x, -pi2, pi2, pi2 / 25}]], _Line|_Polygon]];
  pts = Select[Join @@ (First /@ c), FreeQ[#, Infinity | Indeterminate]&];
  range = {{Min[#], Max[#]}&[First /@ pts], {Min[#], Max[#]}&[Last /@ pts]};
  range += ((First[Differences[#]] / 8 * {-1, 1})& /@ range);
  tr = Trace /. {opts} /. Options[AnimateMP];
  tr = Switch[tr,
    None, {},
    {_, _}, Join[{Red}, TraceMP[#, t, tr, range]& /@ Union[Expand[{args}]], {Black}],
    _, Throw["Bad value for option Trace."]];
  opts1 = Append[DeleteCases[Join[{opts}, Options[AnimateMP]], _[Style|Polygon|Trace, _]], PlotRange -> range];
  Format[x] := ArcTan["t"];
  Return[Animate[
    TableForm[{"t = " <> ToString[Tan[x]], Graphics[Flatten[{tr, #[Tan[x]]& /@ fs}], Sequence @@ opts1]}],
    {x, -pi2, pi2}, AnimationRunning -> False]];
];

(* The Flip procedure *)
Flip::usage = "Flip[p1, p2] performs the flip procedure. Note that input and " <>
"output are pairs of linear motion polynomials, of the form t - k, k in K, " <>
"as opposed to the paper, where only the k's are given / returned.";
Flip[MP[z1_, w1_], MP[z2_, w2_]] :=
Module[{w3, w4, den},
  den = SimpMP[MP[z1 - Conjugate[z2], 0]][[1]];
  If[den === 0, Throw["Flip: primal parts are conjugate to each other."]];
  w3 = (w1 * 2*I*Im[z2] + w2 * Conjugate[z1 - z2]) / den;
  w4 = (w1 * (z1 - z2) + w2 * 2*I*Im[z1]) / den;
  Return[SimpMP /@ {MP[z2, w3], MP[z1, w4]}];
];

(* Construct a linkage, in form of an enriched link graph, from a factored motion polynomial. *)
(* This is described in Algorithm 4 ConstructStrongLinkage in the paper. *)
ConstructLinkage::usage = "ConstructLinkage[mp, l1] constructs a linkage " <>
"of mobility one that realizes the motion given by mp. The motion polynomial " <>
"mp must be given in factored form, and l1 is a linear bounded motion polynomial. " <>
"If l1 is omitted, a suitable such polynomial is taken instead. The output is given " <>
"in the form as needed for ShowLinkage.";
ConstructLinkage[mp_FactoredMP, l1_ : Automatic] :=
Module[{x, n = Length[mp], ks = List @@ mp, kts = {}, ls, kti, li1, eqns = {}, graph,
  len, case, pos},

  (* These values have to be avoided for pp(l1). *)
  ls = Union[#, Conjugate[#]]&[t - First /@ ks];

  If[l1 === Automatic,
    (* If l1 is not given, take one with suitable primal and generic secondary part. *)
    ls = {MP[t - I * First[Complement[Range[n + 1], ls / I]], x]};
  ,
    (* If l1 is given, test part of the IFM condition in advance. *)
    If[MemberQ[ls, t - l1[[1]]], Throw["ConstructLinkage: Condition IFM is not fulfilled."]];
    ls = {l1};
  ];

  (* Compute the l_i and \tilde{k}_i. *)
  Do[
    (* Test the remaining part of the IFM condition. *)
    AppendTo[eqns, FixPoint[ls[[i]]] - FixPoint[ks[[i]]]];
    If[Last[eqns] === {0, 0}, Throw["ConstructLinkage: Condition IFM is not fulfilled."]];
    {kti, li1} = Flip[ls[[i]], ks[[i]]];
    AppendTo[kts, kti];
    AppendTo[ls, li1];
  , {i, n}];

  (* For sp(l1) choose the smallest natural number such that IFM holds. *)
  If[l1 === Automatic,
    li1 = First[Complement[Range[0, n], Flatten[(x /. Solve[Thread[# == 0], x])& /@ eqns]]];
    {ls, kts} = {ls, kts} /. x -> li1;
  ];

  (* Note that we use a different labeling than in the paper (Algorithm 4). *)
  (* We start on the right with 1 (upper row) and n+2 (lower) and increase  *)
  (* the labels while going to the left. This way, the base has label 1. *)
  graph = Join[
    Table[{i, i+1, ks[[n-i+1]]}, {i, n}],
    Table[{n+i+1, n+i+2, kts[[n-i+1]]}, {i, n}],
    Table[{i, n+i+1, ls[[n-i+2]]}, {i, n+1}]];

(*
  (* Determine an order on the links such that each square of the link graph is collision-free. *)
  (* However, in practice, we get fewer collisions by just stacking each square on top of the previous one. *)
  len[k1_, k2_, k3_] :=
    Norm[Differences[FixPoint[Last[First[Cases[graph, {#1, #2, _} | {#2, #1, _}]]]]& @@@ {{k1, k2}, {k2, k3}}]];
  case = len[2, 1, n + 2] > len[1, n + 2, n + 3];
  ls = {1, 2}; pos = 2;
  Do[
    If[len[i, i - 1, i + n] > len[i + n + 1, i + n, i - 1],
      If[case, pos++];
      ls = Insert[ls, i, pos];
    ,
      If[Not[case], pos++];
      ls = Insert[ls, i, pos];
    ];
  , {i, 3, n + 1}];
  Print[Flatten[If[case, {#, # + n + 1}, {# + n + 1, #}]& /@ ls]];
*)
  Return[graph];
];

(* Graphics primitives for a link from {x1, y1} to {x2, y2} of radius r and color c. *)
LineToGraphics[{{x1_, y1_}, {x2_, y2_}}, r_, c_] :=
Module[{vec, rect},
  vec = Together[{x2 - x1, y2 - y1}];
  If[vec === {0, 0}, Return[{}]];
  vec = {vec[[2]], -vec[[1]]} / Sqrt[Together[vec[[1]]^2 + vec[[2]]^2]] * r;
  rect = Together[{{x1, y1} + vec, {x1, y1} - vec, {x2, y2} - vec, {x2, y2} + vec, {x1, y1} + vec}];
  Return[{c, Polygon[rect], Black, Line[rect]}];
];

(* Animate a linkage. *)
ShowLinkage::usage = "ShowLinkage[graph, base] creates an animation of a linkage. " <>
"The link graph must be given in the form {{i, j, mij}, ...} where i and j are the " <>
"labels of two links and mij is a linear bounded motion polynomial describing the relative " <>
"position between these two links. The second argument, base, is the label of the base link " <> 
"(the one that doesn't move). The following options can be given:\n" <>
"ColorOutput -> {1 -> Yellow, 2 -> Green, ...}: to draw link 1 in yellow etc.\n" <>
"Thickness -> 0.01: thickness of links. Thickness -> {0.01, 0.001}: thickness of links and lines.\n" <>
"Trace -> {i, {x, y}}: trace the point {x, y} under the motion of link i.\n" <>
"Direction -> {x, y}: draw the curve with a pen whose shape is a line from {0, 0} to {x, y}.\n" <>
"Range -> {a, b}: range for the time ArcTan[t], i.e., [a, b] \[SubsetEqual] [-\[Pi]/2, \[Pi]/2].\n" <>
"Links -> {i, j, ...}: specify an order in which the links should be drawn.\n" <>
"Style -> \"CompleteGraph\", Style -> \"Star\": how to draw links that are connected with more than 2 joints.\n" <>
"Return -> \"animation\": produces an animation of the linkage.\n" <>
"Return -> {\"picture\", x}: produces an image showing the linkage at time x = ArcTan[t].\n" <>
"Return -> {\"movie\", n}: creates a sequence of n image files that can be used to make a movie.\n" <>
"Return -> {\"movie\", {t1, t2, ..., tn}}: as before, using time ti for frame i.\n" <>
"Return -> \"graph\": draws the link graph.\n" <>
"Return -> \"collisions\": returns a list of the form {{{i, k, j}, s, t}, ...} which means that " <>
"the joint connecting i and j collides with link k at time t at position 0 <= s <= 1.\n" <>
"Return works similarly with \"animation3D\", \"picture3D\", and \"movie3D\".\n" <>
"Moreover, all options of the Graphics / Graphics3D commands can be used.";
Options[ShowLinkage] = {ColorOutput -> {}, PlotRange -> All, Return -> "animation",
  Thickness -> Automatic, Trace -> None, Direction -> {0, 0}, Range -> {-Pi/2 + 0.001, Pi/2 - 0.001},
  Links -> Automatic, Style -> "CompleteGraph"};
ShowLinkage[graph_List, opts:((_Rule|_RuleDelayed)...)] := ShowLinkage[graph, 1, opts];
ShowLinkage[graph1_List, base : Except[_Rule | _RuleDelayed], opts:((_Rule|_RuleDelayed)...)] :=
Module[{color, range, return, thick, trace, dir, r1, r2, draw3D, opts1,
  x, l1, l2, mp, graph, links, joints, lines, psj, js, gr, thk, traceZ, v, td, col},

  (* Deal with options. *)
  {color, range, return, thick, trace, dir, {r1, r2}} =
    {ColorOutput, PlotRange, Return, Thickness, Trace, Direction, Range}
    /. {opts} /. Options[ShowLinkage];
  opts1 = DeleteCases[{opts},
    Rule[ColorOutput | PlotRange | Return | Thickness | Trace | Direction | Range | Links | Style, _]];
  If[Not[MatchQ[range, All | {{_, _}, {_, _}} | {{_, _}, {_, _}, {_, _}}]],
    Throw["Option PlotRange must be of the form {{x1, x2}, {y1, y2}} or {{x1, x2}, {y1, y2}, {z1, z2}}."]];
  If[Not[MatchQ[trace, None | {_, {_, _}}]],
    Throw["Option Trace must be of the form {l, {x, y}}."]];
  draw3D = MatchQ[return, "animation3D" | {"picture3D" | "movie3D", _}];

  (* For each edge (i,j) add the opposite edge (j,i) to the graph. *)
  (* Note that MP[z, w] ** MP[conj[z], -w] = a^2 + b^2 + 2*a*t + t^2. *)
  graph = Join[graph1, {#2, #1, If[Head[#3] === MP, MP[t + Conjugate[#3[[1]] - t], -#3[[2]]], #3]}& @@@ graph1];

  (* For each link, compute a motion polynomial that describes its absolute motion. *)
  (* For each joint, compute its initial position (t = infinity). *)
  links = {{base, 1 + eta * 0}};
  joints = psj = {};
  While[graph =!= {},
    {l1, l2, mp} = First[Select[graph, MemberQ[First /@ links, First[#]]&]];
    If[Head[mp] === MP, v = FixPoint[mp], AppendTo[psj, {l1, l2}]; v = mp; mp = 1];
    mp = mp ** First[Cases[links, {l1, a_} :> a]];
    If[Not[MemberQ[First /@ links, l2]], AppendTo[links, {l2, mp}]];
    AppendTo[joints, {l1, l2, Together[ActR2[mp, v]]}];
    graph = DeleteCases[graph, {l1, l2, _} | {l2, l1, _}];
  ];
  If[trace =!= None,
    mp = First[Cases[links, {trace[[1]], a_} :> a]];
    AppendTo[joints, {trace[[1]], trace[[1]], Together[ActR2[mp, trace[[2]]]]}];
  ];
  l1 = Links /. {opts} /. Options[ShowLinkage];
  links = If[l1 === Automatic, SortBy[links, First], First[Cases[links, {#, _}]]& /@ l1];

  If[return === "graph",
    {l1, l2} = {Take[##], Drop[##]}&[Sort[First /@ links], Ceiling[Length[links] / 2]];
    If[MemberQ[l2, base], {l1, l2} = {l2, l1}];
    v = Function[l, If[MemberQ[l1, l], {-Position[l1, l][[1,1]], 1}, {-Position[l2, l][[1,1]], 0}]];
    thk = If[MatchQ[thick, {_, _}], thick[[2]], 0.002];
    gr = Join[{Thickness -> thk}, Line[{v[#1], v[#2]}]& @@@ joints];
    AppendTo[opts1, ImageSize -> 400];
    thk = (ImageSize /. opts1) / 6 / Length[l1];
    Do[
      x = links[[i,1]];
      col = If[x =!= (x /. color), x /. color, White];
      gr = Join[gr, {col, Disk[v[x], 1/8], Black, Circle[v[x], 1/8], Text[Style[x, thk], v[x]]}];
    , {i, Length[links]}];
    Return[Graphics[gr, Sequence @@ opts1]];
  ];

  (* Find collisions. *)
  If[return === "collisions",
    col = Select[Subsets[First /@ links, {3}], MemberQ[joints, {#[[1]], #[[3]], _} | {#[[3]], #[[1]], _}]&];
    (* {i1, i3, i2} means that link i3 and the joint between links i1 and i2 could collide. *)
    col = Flatten[Function[{i1, i3, i2},
      js = Last[First[Cases[joints, {i1, i2, _} | {i2, i1, _}]]];
      lines = Subsets[Last /@ Cases[joints, {i3, _, _} | {_, i3, _}], {2}];
      {{i1, i3, i2}, (x * #[[1]] + (1 - x) * #[[2]] - js)}& /@ lines] @@@ col, 1];
    col = Flatten[Function[{i123, sys},
      Prepend[{x, t} /. #, i123]& /@
      Join[
        Solve[Thread[sys == 0], {x, t}],
        Append[#, t -> Infinity]& /@ Solve[Thread[Limit[sys, t -> Infinity] == 0], x]]
      ] @@@ col, 1];
    col = Select[col, Element[#[[2]], Reals] &];
    col = Select[col, 0 <= #[[2]] <= 1 && (Element[#[[3]], Reals] || #[[3]] === Infinity) &];
    Return[col];
  ];

  (* Determine the plot range (if not given by option). *)
  If[range === All,
    js = With[{r = Pi/2 - 0.01}, Union @@ Table[(Last /@ joints) /. t -> Tan[x], {x, -r, r, r / 25}]];
    range = {{Min[#], Max[#]}&[First /@ js], {Min[#], Max[#]}&[Last /@ js]};
    range += ((First[Differences[#]] / 8 * {-1, 1})& /@ range);
    (* Avoid images of small width and large height. *)
    With[{dx = range[[1,2]] - range[[1,1]], dy = range[[2,2]] - range[[2,1]]},
      If[dy > 2 * dx, range[[1]] = range[[1,1]] + dx / 2 + {-1, 1} * dy / 2];
    ];
  ];

  (* Determine the thickness for drawing links and lines (if not given by option). *)
  thick = If[thick === Automatic, {Max[Differences /@ range] / 400, 0.002}, Flatten[{thick, 0.002}]];
  thk = thick[[1]];
  gr = {Thickness[thick[[2]]]};

  (* Graphics primitives for the traced curve *)
  If[trace =!= None,
    mp = Cases[links, {trace[[1]], _}][[1,2]];
    traceZ = If[draw3D, Position[links, {trace[[1]], _}][[1,1]] * 8 * thk, False];
    (* They are generated separately in the case of a movie. *)
    If[Not[MatchQ[return, {"movie" | "movie3D", _}]],
      gr = Join[gr, {Red, Flatten[TraceMP[mp, t, trace[[2]], Take[range, 2], {r1, r2}, dir, traceZ]]}];
    ];
  ];

  (* Graphics primitives for links and joints *)
  links = First /@ links;
  Do[
    l1 = links[[i]];
    js = Cases[joints, {l1, _, _} | {_, l1, _}];

    (* Draw a link. *)
    lines = Switch[Style /. {opts} /. Options[ShowLinkage],
      "CompleteGraph", Subsets[Last /@ js, {2}],
      "Star", With[{pts = Last /@ js}, If[(l2 = Length[pts]) === 2, {pts}, {Total[pts] / l2, #}& /@ pts]]
    ];
    col = If[l1 =!= (l1 /. color), l1 /. color, If[draw3D, Gray, LightGray]];
    gr = Join[gr, Flatten[If[draw3D,
      (* 3D *) {col, EdgeForm[], Specularity[White, 30], Cylinder[Join[#, {{i}, {i}} * 8 * thk, 2], 2 * thk]},
      (* 2D *) LineToGraphics[#, 3/2 * thk, col]
      ]& /@ lines]];

    (* Draw only those joints which are not attached to a yet-to-be-drawn link (relevant only in 2D). *)
    js = Select[js, Intersection[Most[#], Drop[links, i]] === {} &];
    If[draw3D,
      js = Function[j,
        pos = Append[j[[3]], #]& /@ (Sort[Position[links, #][[1,1]]& /@ Take[j, 2]] * 8 * thk);
        (* Hack for nice display of Z-joints *)
        (* If[MatchQ[Take[j, 2], {-4, 9} | {-3, 8}], pos = Drop[pos, 1]]; *)
        Which[
          (* pen *) SameQ @@ pos && Length[pos] == 2,
            {RGBColor[0.99, 0, 0], Specularity[White, 30], Sphere[pos[[1]], 4 * thk]},
          (* pseudo-joint (rigid) *) MemberQ[psj, Take[j, 2]],
            col = If[l1 =!= (l1 /. color), l1 /. color, Gray];
            Join[{col, EdgeForm[], Specularity[White, 30], Cylinder[pos, 2 * thk]}, Sphere[#, 2 * thk]& /@ pos],
          (* Z-joint *) Length[pos] == 1,
            {Gray, EdgeForm[Thin], Specularity[White, 10], Cylinder[{pos[[1]] + {0, 0, -2 * thk}, pos[[1]] + {0, 0, 2 * thk}}, 3 * thk]},
          (* joint *) True,
            pos = {{pos[[1]] - {0, 0, 1} * 3 * thk, Total[pos] / 2}, {Total[pos] / 2, pos[[2]] + {0, 0, 1} * 3 * thk}};
            {Gray, EdgeForm[Thin], Specularity[White, 10], Cylinder[#, 3 * thk]& /@ pos}
        ]
      ] /@ js;
    ,
      js = With[{pen = (trace =!= None && #1 === #2 === trace[[1]])},
        {If[pen, RGBColor[0.99, 0, 0], LightGray],
         Disk[#3, 4 * thk], Black, Circle[#3, 4 * thk],
         If[pen, {}, Disk[#3, thk]]}]& @@@ js;
    ];
    gr = Join[gr, Flatten[js]];
  , {i, Length[links]}];

  (* Options for Graphics *)
  If[draw3D,
    If[Length[range] === 2, range = Append[range, {0, Length[links] + 1} * 8 * thk]];
    AppendTo[opts1, Lighting -> "Neutral"];
  ];
  opts1 = Sequence @@ Join[{PlotRange -> range}, opts1, {ImageSize -> 400}];

  draw3D = If[draw3D, Graphics3D, Graphics];
  Switch[return,
  "animation" | "animation3D",
    Format[x] := ArcTan["t"];
    gr = (Function @@ {t, "cmnd"[gr, opts1]}) /. "cmnd" -> draw3D;
    Return[Animate[TableForm[{"t = " <> ToString[Tan[x]], gr[Tan[x]]}], {x, r1, r2}, AnimationRunning -> False]],
  {"picture" | "picture3D", _?NumericQ},
    Return[draw3D[gr /. t -> Tan[return[[2]]], opts1]],
  {"movie" | "movie3D", _Integer | _List},
    {l1, l2} = 2 * Round[ImageDimensions[draw3D[gr /. t -> 0, opts1]] / 4];
    If[Head[td = return[[2]]] === Integer, td = Table[r1 + (r2-r1)*(i+1/2)/td, {i, 0, td - 1}]];
    Do[
      graph = If[trace === None, {}, Flatten[{Red, TraceMP[mp, t, trace[[2]], range, {td[[1]], td[[i]]}, dir, traceZ]}]];
      Export[
        "temp" <> (StringJoin @@ Table["0", {5 - StringLength[#]}]) <> # <> ".jpg" &[ToString[i-1]],
        ImageResize[draw3D[Join[{gr[[1]]}, graph, Rest[gr] /. t -> Tan[td[[i]]]], opts1], {l1, l2}],
        "CompressionLevel" -> 0.01];
    , {i, Length[td]}];
    Return["In the directory " <> Directory[] <> " execute the following command:\n" <>
      "ffmpeg -i temp%05d.jpg -r 25 -qscale 4 movie.mp4"],
  _,
    Throw["Option Return must be one of \"animation\", {\"picture\", t}, {\"movie\", frames}."];
  ];
];
