# Konvertierung Vektor zu symmetrischer Matrix
# Einleitung
# Generiere die Umwandlung von Vektor mit oberem rechten Teil einer Symmetrischen Matrix zur Matrix selbst für die Dimensionen des Roboters.
# Diese Umwandlung verursacht weniger Rechenoperationen als die Verwendung der numerischen Funktion vec2symmat in Matlab, da dort jedes Mal die Indizes beim Zusammenbauen der Matrix getestet werden müssen.
# Autor
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-12
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(ArrayTools):
with(codegen):
with(CodeGeneration):
with(StringTools):
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
read "../helper/proc_MatlabExport":
read "../helper/proc_vector2symmat":
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
printf("Generiere Symmat2Vector-Funktionen für %s\n", robot_name):
codegen_opt := 0: # Soll nicht von Einstellung in robot_env überschrieben werden.
;
# Funktion symmat2vector für den Roboter definieren
# Erstelle eine Dummy-Variable (mv), die als temporäre Variable in Matlab dient (zum Zusammensetzen der Matrix).
clear('mv'):
M_NQ:= vec2symmat(mv, NQ):
M_NQJ:= vec2symmat(mv, NQJ):
M_6:= vec2symmat(mv, 6):
# Warnungen bei Code-Generierung unterdrücken. Die Meldung das der Ausdruck mv() in Matlab nicht bekannt ist, spielt keine Rolle, da diese Variable nach dem Einsetzen des Codes vorher definiert sein wird.
interface(warnlevel=0):
MatlabExport(M_NQ, sprintf("../codeexport/%s/tmp/vec2symmat_%d_matlab.m", robot_name, NQ), codegen_opt):
MatlabExport(M_NQJ, sprintf("../codeexport/%s/tmp/vec2symmat_%d_matlab.m", robot_name, NQJ), codegen_opt):
MatlabExport(M_6, sprintf("../codeexport/%s/tmp/vec2symmat_%d_matlab.m", robot_name, 6), codegen_opt):


######################################################################
# Binary operations on finite sets

`is_element/binops` := (A::set,B::set,C::set) -> proc(p)
 local AB,dom,cod,a,b;
 global reason;

 if not(type(p,table)) then
  reason := [convert(procname,string),"not table"];
  return false;
 fi;

 AB := {seq(seq([a,b],b in B),a in A)};

 if {indices(p)} <> AB then
  reason := [convert(procname,string),"wrong domain",{indices(p)},AB];
  return false;
 fi;

 cod := map(op,{entries(p)});
 if cod minus C <> {} then
  reason := [convert(procname,string),"wrong codomain",cod,C];
  return false;
 fi;

 return true;
end;

`is_equal/binops` := (A::set,B::set,C::set) -> proc(p,q)
 local a,b;
 global reason;

 for a in A do
  for b in B do
   if p[a,b] <> q[a,b] then
    reason := [convert(procname,string),"different value",a,b,p[a,b],q[a,b]];
    return false;
   fi;
  od;
 od;

 return true;
end;

`is_leq/binops` := NULL;

`random_element/binops` := (A::set,B::set,C::set) -> proc()
 local p,n,r,a,b;

 p := table();
 if B = {} then
  if A = {} then
   return eval(p);
  else
   return FAIL;
  fi;
 fi;

 n := nops(C);

 r := rand(1..n);

 for a in A do
  for b in B do
   p[a,b] := C[r()];
  od;
 od;

 return eval(p);
end;

`list_elements/binops` := proc(A::set,B::set,C::set)
 local nA,nB,nC,J,L,i,j,k,u;

 nA := nops(A);
 nB := nops(B);
 nC := nops(C);
 J := [seq(seq([A[i],B[j]],j=1..nB),i=1..nA)];
 L := [[]];
 for i from 1 to nA*nB do
  L := [seq(seq([op(u),j],j=1..nC),u in L)];
 od:

 L := map(u -> table([seq(op(J[k])=C[u[k]],k=1..nA*nB)]),L);
 return L;
end;

`count_elements/binops` := (A::set,B::set,C::set) -> nops(C) ^ (nops(A)*nops(B));

`is_associative/binops` := (A::set) -> proc(m)
 local a,b,c;
 for a in A do
  for b in A do
   for c in A do
    if m[a,m[b,c]] <> m[m[a,b],c] then
     return false;
    fi;
   od;
  od;
 od;
 return true;
end:

`is_commutative/binops` := (A::set) -> proc(m)
 local a,b;
 for a in A do
  for b in A do
   if m[a,b] <> m[b,a] then
    return false;
   fi;
  od;
 od;
 return true;
end:

`is_left_proj/binops` := (A::set) -> proc(m)
 local a,b;
 for a in A do
  for b in A do
   if m[a,b] <> a then
    return false;
   fi;
  od;
 od;
 return true;
end:

`is_right_proj/binops` := (A::set) -> proc(m)
 local a,b;
 for a in A do
  for b in A do
   if m[a,b] <> b then
    return false;
   fi;
  od;
 od;
 return true;
end:

`is_idempotent/binops` := (A::set) -> proc(m)
 local a;
 for a in A do
  if m[a,a] <> a then
   return false;
  fi;
 od;
 return true;
end:

`is_left_neutral/binops` := (A::set) -> proc(m,e)
 local a;

 if not(member(e,A)) then return false; fi;
 for a in A do
  if m[e,a] <> a then return false; fi;
 od:
 return true;
end:

`is_right_neutral/binops` := (A::set) -> proc(m,e)
 local a;

 if not(member(e,A)) then return false; fi;
 for a in A do
  if m[a,e] <> a then return false; fi;
 od:
 return true;
end:

`is_neutral/binops` := (A::set) -> proc(m,e)
 return `is_left_neutral/binops`(A)(m,e) and
        `is_right_neutral/binops`(A)(m,e);
end:

`has_left_neutral/binops` := (A::set) -> proc(m)
 local e;

 for e in A do
  if `is_left_neutral/binops`(A)(m,e) then
   return true;
  fi;
 od:
 return false;
end:

`has_right_neutral/binops` := (A::set) -> proc(m)
 local e;

 for e in A do
  if `is_right_neutral/binops`(A)(m,e) then
   return true;
  fi;
 od:
 return false;
end:

`has_neutral/binops` := (A::set) -> proc(m)
 local e;

 for e in A do
  if `is_neutral/binops`(A)(m,e) then
   return true;
  fi;
 od:
 return false;
end:

`is_left_absorbing/binops` := (A::set) -> proc(m,e)
 local a;

 if not(member(e,A)) then return false; fi;
 for a in A do
  if m[e,a] <> e then return false; fi;
 od:
 return true;
end:

`is_right_absorbing/binops` := (A::set) -> proc(m,e)
 local a;

 if not(member(e,A)) then return false; fi;
 for a in A do
  if m[a,e] <> e then return false; fi;
 od:
 return true;
end:

`is_absorbing/binops` := (A::set) -> proc(m,e)
 return `is_left_absorbing/binops`(A)(m,e) and
        `is_right_absorbing/binops`(A)(m,e);
end:

`has_left_absorbing/binops` := (A::set) -> proc(m)
 local e;

 for e in A do
  if `is_left_absorbing/binops`(A)(m,e) then
   return true;
  fi;
 od:
 return false;
end:

`has_right_absorbing/binops` := (A::set) -> proc(m)
 local e;

 for e in A do
  if `is_right_absorbing/binops`(A)(m,e) then
   return true;
  fi;
 od:
 return false;
end:

`has_absorbing/binops` := (A::set) -> proc(m)
 local e;

 for e in A do
  if `is_absorbing/binops`(A)(m,e) then
   return true;
  fi;
 od:
 return false;
end:


rename_cohomology_gens := proc(T0::table,u::list)
 local u0,n0,r,T,i;

 u0 := [op(T0["cohomology_gens"])];
 n0 := nops(u0);

 if nops(u) <> n0 then
  error("Incorect number of cohomology generators");
 fi;

 r := [seq(u0[i] = u[i],i=1..n0)];

 T := copy(T0);
 T["cohomology_gens"] := subs(r,T0["cohomology_gens"]);
 T["cohomology_rels"] := subs(r,T0["cohomology_rels"]);
 T["cohomology_degrees"] := table([]):
 for i from 1 to n0 do
  T["cohomology_degrees"][u[i]] :=
   T0["cohomology_degrees"][u0[i]];
 od;
 T["cohomology_basis"] := subs(r,T0["cohomology_basis"]);
 for i from 0 to T["dimension"] do
  T["cohomology_graded_basis"][i] :=
   subs(r,T["cohomology_graded_basis"][i]);
 od:

 return eval(T);
end:

make_product_space := proc(T0,T1)
 local T,d0,d1,B0,B1,B,i0,i1,u0,u1,x,i;
 T := table():

 T["name"] := sprintf("%s x %s",T0["name"],T1["name"]);
 d0 := T0["dimension"];
 d1 := T1["dimension"];
 T["dimension"] := d0+d1;
 T["cohomology_gens"] := tdeg(op(T0["cohomology_gens"]),
                              op(T1["cohomology_gens"]));
 T["cohomology_rels"] := [op(T0["cohomology_rels"]),
                          op(T1["cohomology_rels"])];
 T["cohomology_degrees"] := table([]);
 for x in T0["cohomology_gens"] do
  T["cohomology_degrees"][x] := T0["cohomology_degrees"][x];
 od:
 for x in T1["cohomology_gens"] do
  T["cohomology_degrees"][x] := T1["cohomology_degrees"][x];
 od:

 T["cohomology_basis"] := [seq(seq(u0*u1,u0 in T0["cohomology_basis"]),
                                         u1 in T1["cohomology_basis"])];

 B0 := T0["cohomology_graded_basis"]:
 B1 := T1["cohomology_graded_basis"]:
 B := table():

 for i from 0 to d0+d1 do
  B[i] := NULL:

  for i0 from max(0,i-d1) to min(i,d0) do
   i1 := i - i0;
   B[i] := B[i],seq(seq(u0*u1,u0 in B0[i0]),u1 in B1[i1]);
  od:
 od;

 T["cohomology_graded_basis"] := eval(B):

 return eval(T);
end:


(*
    变量可以划分为：
    + 自由变量：在满足非零的前提下可以全部取0。
    + 确定性变量：取值已经确定，或者由其它变量确定。
    + 有约束变量：其约束来自于解的定义域，由解的性质可知，其值不由其它变量所确定。

    处理时主要处理约束条件，自由变量可以加单取0，确定性变量的值已知。在完全处理约束的前提下，无需验证是否满足约束。

    至于如何处理约束：
    + 和c有关的约束：为了能够始终满足约束，特解应当和c有关，可以采取变成等式约束进行求解的方式进行处理。(注意对于单独的c的约束，直接删除)
    + 和c无关的约束：可以简单求解不等式，求解后的结果，都是单边的不等式约束，可以看情况取解。

    对于求解后的结果，可以细分为：
    + 单变量约束，可以按照简单的方法取定特解。
    + 多变量约束，可以在取定其它变量的值之后，转化单变量约束进行处理。
*)

$ifndef _FETCH_
$define _FETCH_

$include "Condition.mpl"
$include "Utils.mpl"

# 根据方程和约束取特解
fetchSpecSol:=proc(sol::list,con::set,{nonzero::boolean:=false})
    local vars,vf,eq,ca,cc,res,ccv,sc;
    vars:=lhs~(sol);
    vf,eq,ca,cc:=filtCon({sol[],classifySolve(con)[]});
    if cc<>{} then
        # 对于和c有关的不等式约束，转化成等式约束后再进行求解
        cc:=transIeq~(cc);
        ccv:=indets(cc,name);
        ccv:=select(type,ccv,specindex(a));
        sc:=RealDomain:-solve(cc,[ccv[]],explicit);
        res:=map(rebuildAndFetch,sc,vars,vf,eq,ca,nonzero);
        res:=sortByComplexity(res);
        return res[1];
    else
        return fetchByCons(vars,vf,eq,ca,nonzero);
    end if;
end proc:

# 转化关于c的不等式约束之后，再对约束进行分类
rebuildAndFetch:=proc(s,vars,_vf,_eq,_ca,nonzero)
    local vf,eq,ca,cc;
    vf,eq,ca,cc:=filtCon({s[]} union findSolutionDomain(s));
    vf:=_vf minus indets(lhs~(eq),name);
    eq:=eq union _eq;
    ca:=ca union _ca;
    return fetchByCons(vars,vf,eq,ca,nonzero);
end proc:

# 求解关于a的约束之后，转化为单边约束，再逐解取特解
fetchByCons:=proc(vars,vf,eq,ca,nonzero)
    local sca,res;
    sca:=[RealDomain:-solve(ca)];
    res:=map[4](fetchBySingleCons,vars,vf,eq,sca,nonzero);
    if nonzero then
        # 在满足非零要求时，目的是给出所有可能的基
        # 而求解a的约束产生的每个解是不相交的
        # 所以需要合并各个解的基
        return `union`(res[]);
    else
        return sortByComplexity(res)[1];
    end if;
end proc:

# 对于和a有关的约束，采用逐步替换的方法取特解
fetchBySingleCons:=proc(vars,_vf,_eq,_ca,nonzero)
    local vf,eq,ca,sca,res,req,tmp,i,n,ra;
    vf:=_vf;# 自由变量
    eq:=_eq;# 不等式方程的解等式约束
    ca:=_ca;# 求解跟a有关的约束得到的解
    # 对于单变量约束，采用最邻近整数方式取特解
    # 逐步替换多变量约束中的变量，最终都取得特解
    while true do
        sca,ca:=selectremove(isSingleCon,ca);
        if sca={} then
            break;
        end if;
        sca:=fetchIeq~(sca);
        eq:=eq union sca;
        ca:=subs(sca[],ca);
    end do;
    # 由于对不等约束的求解可能存在等式约束，因此最后剩下的是等式约束
    # BUG  出错的问题在于上面得到的ca可能会有等式约束
    ca,ra:=selectremove(type,ca,equation);
    vf:={seq(x=0,x in vf)};
    eq:=eq union ca union vf;
    req:=vars;
    # 带入两次是因为带入一次可能不完全
    res:=eval(subs(eq[],req));
    res:=eval(subs(eq[],res));
    # 假设还有变量存在，就重新求解
    if indets(res,specindex(a))<>{} then
        res:=fetchSpecSol([seq(a[i]=res[i],i=1..numelems(vars))],{});
    end if;
    if nonzero then
        if andmap(type,res,0) then
            n:=numelems(vf);
            res:={};
            for i from 1 to n do
                tmp:=subsop([i,2]=1,vf);
                res:=res union {eval(subs(tmp[],eq[],req))};
            end do;
        else
            res:={res};
        end if;
    end if;
    return res;
end proc:

# 是否是单变量约束
isSingleCon:=proc(x)
    return not type(x,`=`) and numelems(indets(x,name))=1;
end proc:

# 筛选：自由变量，确定性约束，和c无关的不等式约束，和c有关的不等式约束
# 会删除之和c有关的约束
filtCon:=proc(con::set)
    local ef,eq,ca,cc,rst,vf;
    ef,rst:=selectremove(x->lhs(x)=rhs(x),con);
    eq,rst:=selectremove(type,rst,`=`);
    vf:=indets(ef,name) minus lhs~(eq) minus indets(rst,name);
    cc,ca:=selectremove(has,rst,c);
    cc:=select(has,cc,a);
    return vf,eq,ca,cc;
end proc:

# 将不等式约束转化为等式约束
transIeq:=proc(x)
    if op(0,x)=`<>` or op(0,x)=`<` then
        return lhs(x)=rhs(x)-1;
    elif op(0,x)=`>` then
        return lhs(x)=rhs(x)+1;
    else
        return lhs(x)=rhs(x);
    end if;
end proc:

# 对于 <> < <= 的约束条件取特解
# 取满足条件的最邻近整数
fetchIeq:=proc(x)
    local r;
    if type(x,`<>`) then
        r:=lhs(x)=floor(rhs(x))+1;
    elif type(lhs(x),numeric) then
        if type(x,`<`) then
            r:=rhs(x)=floor(lhs(x))+1;
        else
            r:=rhs(x)=ceil(lhs(x));
        end if;
    else
        if type(x,`<`) then
            r:=lhs(x)=ceil(rhs(x))-1;
        else
            r:=lhs(x)=floor(rhs(x));
        end if;
    end if;
end proc:

$endif


#######################################################################
# This file is part of the crlibm library, and is distributed under
# the  LGPL.
# To use:
# restart; read "exp-td.mpl";
Digits := 145:

interface(quiet=true):

read "common-procedures.mpl":
read "triple-double.mpl":
read "gal.mpl":
mkdir("TEMPASIN"):

with(orthopoly,T):

truncPoly := proc(p,k)
local i, q:
q := 0:
convert(q,polynom):
for i from 0 to min(degree(p,x),k) do
    q := q + coeff(p,x,i) * x^i:
od:
return (q):
end:

intervals := 10:

for i from 1 to intervals do
	epsAccurate[i] := 0.5:
	epsQuick[i] := 0.5:
	polyAccurate[i] := 1:
	polyQuick[i] := 1:
od:
epsAccurateSpecial := 0.5:
epsQuickExtra := 0.5:

lowestIntervalMax := 0.185:
highestIntervalMin := 0.78:

polyAccurateTDCoeffsLowest := 13:
polyAccurateDDCoeffsLowest := 12:
polyAccurateDCoeffsLowest := 16:

polyAccurateDegreeLowest := polyAccurateTDCoeffsLowest + polyAccurateDDCoeffsLowest + polyAccurateDCoeffsLowest:

polyQuickDDCoeffsLowest := 12:
polyQuickDCoeffsLowest := 10:

polyQuickDegreeLowest := polyQuickDDCoeffsLowest + polyQuickDCoeffsLowest:

polyAccurateTDCoeffsMiddle := 7:
polyAccurateDDCoeffsMiddle := 9:
polyAccurateDCoeffsMiddle := 19:

polyAccurateDegreeMiddle := polyAccurateTDCoeffsMiddle + polyAccurateDDCoeffsMiddle + polyAccurateDCoeffsMiddle:

polyQuickDDCoeffsMiddle := 7:
polyQuickDCoeffsMiddle := 7:

polyQuickDegreeMiddle := polyQuickDDCoeffsMiddle + polyQuickDCoeffsMiddle:

polyAccurateTDCoeffsHighest := 9:
polyAccurateDDCoeffsHighest := 9:
polyAccurateDCoeffsHighest := 11:

polyAccurateDegreeHighest := polyAccurateTDCoeffsHighest + polyAccurateDDCoeffsHighest + polyAccurateDCoeffsHighest:

polyQuickDDCoeffsHighest := 9:
polyQuickDCoeffsHighest := 9:

polyQuickDegreeHighest := polyQuickDDCoeffsHighest + polyQuickDCoeffsHighest:

extrabound := hexa2ieee(["3F500000","00000000"]):
polyQuickDegreeExtra := 5:

bound[0] := 0:
b := nearest(lowestIntervalMax):
he := ieeehexa(b):
bound[1] := hexa2ieee([he[1],"00000000"]):
b := nearest(highestIntervalMin):
he := ieeehexa(b):
bound[intervals - 1] := hexa2ieee([he[1],"00000000"]):
bound[intervals] := 1:
linearWidth := (highestIntervalMin - lowestIntervalMax) / (intervals - 2):
scaleSum := 0:
scale[2] := 1.5:
scale[3] := 1.35:
scale[4] := 1.18:
scale[5] := 1.00:
scale[6] := 0.915:
scale[7] := 0.74:
scale[8] := 0.595:
scale[9] := 0.50:
for i from 2 to (intervals-1) do
	scaleSum := scaleSum + scale[i]:
od:
for i from 2 to (intervals-1) do
	scale[i] := scale[i] / scaleSum;
od:
current := lowestIntervalMax:
for i from 2 to (intervals-2) do
	b := nearest(current + (highestIntervalMin - lowestIntervalMax) * scale[i]):
	current := b:
	he := ieeehexa(b):
	bound[i] := hexa2ieee([he[1],"00000000"]):
od:
printf("Using %d intervals with bounds:\n",intervals):
for i from 0 to intervals do
	printf("bound[%d] = %1.30e\n",i,bound[i]):
od:
printf("Using an extra bound for truncating the quick poly to deg. %d in interval #0 for small args up to %f (2^(%f))\n",
	polyQuickDegreeExtra,extrabound,log[2](abs(extrabound)));

printf("Computing Gal's accurate table values for interval midpoints\n"):
for i from 2 to (intervals-1) do
	m := nearest((bound[i] + bound[i-1]) / 2):
	mhe := ieeehexa(m):
	printf("Interval %d: accurate table research start value %f (%s%s)\n",i,m,mhe[1],mhe[2]):
	midpointFloat[i] := galDoubleToDoubleDouble(nearest(m),arcsin,2^(-121),2^(15)):
od:

printf("Using the following floating point midpoints for intervals 2-%d:\n",intervals-2):
for i from 2 to (intervals-1) do
	mhe := ieeehexa(midpointFloat[i]):
	printf("midpointFloat[%d] = %f (%s%s)\n",i,midpointFloat[i],mhe[1],mhe[2]):
od:

printf("The reduced argument z is therefore bounded by:\n"):
printf("Interval 1: |z| < 2^(%f)\n",
	log[2](abs(bound[1]))):
for i from 2 to (intervals-1) do
	printf("Interval %d: |z| < 2^(%f)\n",i,
	log[2](max(abs(midpointFloat[i] - bound[i-1]),abs(midpointFloat[i] - bound[i])))):
od:
printf("Interval %d: |z| < 2^(%f)\n",intervals,
	log[2](abs(1 - bound[intervals-1]))):

printf("Using a %d degree polynomial for lowest interval (1) (accurate phase)\n",
	polyAccurateDegreeLowest):
printf("with %d triple-double, %d double-double and %d double coefficients\n",
	polyAccurateTDCoeffsLowest, polyAccurateDDCoeffsLowest, polyAccurateDCoeffsLowest):
printf("Using a %d degree polynomial for lowest interval (1) (quick phase)\n",
	polyQuickDegreeLowest):
printf("with %d double-double and %d double coefficients\n",
	polyQuickDDCoeffsLowest, polyQuickDCoeffsLowest):
printf("Using a %d degree polynomial for middle intervals (2-%d) (accurate phase)\n",
	polyAccurateDegreeMiddle,intervals-2):
printf("with %d triple-double, %d double-double and %d double coefficients\n",
	polyAccurateTDCoeffsMiddle, polyAccurateDDCoeffsMiddle, polyAccurateDCoeffsMiddle):
printf("Using a %d degree polynomial for middle intervals (2-%d) (quick phase)\n",
	polyQuickDegreeMiddle,intervals-2):
printf("with %d double-double and %d double coefficients\n",
	polyQuickDDCoeffsMiddle, polyQuickDCoeffsMiddle):
printf("Using a %d degree polynomial for highest interval (%d) (accurate phase)\n",
	polyAccurateDegreeHighest,intervals):
printf("with %d triple-double, %d double-double and %d double coefficients\n",
	polyAccurateTDCoeffsHighest, polyAccurateDDCoeffsHighest, polyAccurateDCoeffsHighest):
printf("Using a %d degree polynomial for highest interval (%d) (quick phase)\n",
	polyQuickDegreeHighest,intervals):
printf("with %d double-double and %d double coefficients\n",
	polyQuickDDCoeffsHighest, polyQuickDCoeffsHighest):

if true then

printf("Computing polynomials for interval 1 ([%f;%f])\n",bound[0],bound[1]):

f := unapply(convert(series((arcsin(sqrt(x))/sqrt(x) - 1)/x,x=0,300),polynom),x):

p := unapply(eval(numapprox[chebyshev](f(x),x=(bound[0])^2..(bound[1])^2,2^(-127))),x):

polyAccuExact[1] := truncPoly(subs(X=x^2,p(X))*x^3+x,polyAccurateDegreeLowest):

polyAccurate[1] := poly_exact32(polyAccuExact[1],polyAccurateTDCoeffsLowest,polyAccurateDDCoeffsLowest):

epsAccurate[1] := numapprox[infnorm]((polyAccurate[1]/arcsin(x))-1, x=bound[0]..bound[1]):
epsAccurateSpecial := numapprox[infnorm]((polyAccurate[1]/arcsin(x))-1, x=bound[0]..evalf(sin(2^(-18)))):

polyQuickExact[1] := truncPoly(polyAccuExact[1],polyQuickDegreeLowest):
polyQuick[1] := poly_exact2(polyQuickExact[1],polyQuickDDCoeffsLowest):

epsQuick[1] := numapprox[infnorm]((polyQuick[1]/arcsin(x))-1, x=bound[0]..bound[1]):

polyQuickExtraExact := truncPoly(polyAccuExact[1],polyQuickDegreeExtra):
polyQuickExtra := poly_exact2(polyQuickExtraExact,polyQuickDegreeExtra):
epsQuickExtra := numapprox[infnorm]((polyQuickExtra/arcsin(x))-1, x=bound[0]..extrabound):

end if:

for i from 2 to (intervals-1) do

printf("Computing polynomials for interval %d ([%f;%f])\n",i,bound[i-1],bound[i]):
printf("Reduced argument z will be in interval [%1.8e;%1.8e] ([-2^%f,2^%f]),\n",
	bound[i-1]-midpointFloat[i],bound[i]-midpointFloat[i],
	log[2](abs(bound[i-1]-midpointFloat[i])),log[2](abs(bound[i]-midpointFloat[i]))):

f := unapply(arcsin(x+midpointFloat[i]),x):

fhelp := unapply(convert(series((f(x)-arcsin(midpointFloat[i]))/x,x=0,polyAccurateDegreeMiddle*3),polynom),x):

polyAccuExact[i] := numapprox[minimax](fhelp(x),
				x=bound[i-1]-midpointFloat[i]..bound[i]-midpointFloat[i],
				[polyAccurateDegreeMiddle,0],1,'err')*x + nearestDD(evalf(arcsin(midpointFloat[i]))):

polyAccurate[i] := poly_exact32(polyAccuExact[i],polyAccurateTDCoeffsMiddle,polyAccurateDDCoeffsMiddle):

epsAccurate[i] := numapprox[infnorm]((polyAccurate[i]/f(x))-1,
					x=bound[i-1]-midpointFloat[i]..bound[i]-midpointFloat[i]):

polyQuickExact[i] := truncPoly(polyAccuExact[i],polyQuickDegreeMiddle):
polyQuick[i] := poly_exact2(polyQuickExact[i],polyQuickDDCoeffsMiddle):

epsQuick[i] := numapprox[infnorm]((polyQuick[i]/f(x))-1,
			 	   x=bound[i-1]-midpointFloat[i]..bound[i]-midpointFloat[i]):

od:

if true then

printf("Computing polynomials for interval %d ([%f;%f])\n",intervals,bound[intervals-1],bound[intervals]):

g := unapply(((arcsin(1 - x) - Pi/2)/sqrt(2*x)),x):
f := unapply(convert(series(((g(x)+1)/x),x=0,polyAccurateDegreeHighest*4),polynom),x):

polyAccuExact[intervals] := numapprox[minimax](f(x),x=(1-bound[intervals]+2^(-53))..(1-bound[intervals-1]),
						[polyAccurateDegreeHighest-1,0],1,'err')*x-1:

polyAccurate[intervals] := poly_exact32(polyAccuExact[intervals],polyAccurateTDCoeffsHighest,polyAccurateDDCoeffsHighest):

epsAccurate[intervals] := numapprox[infnorm](((unapply(polyAccurate[intervals],x)(x)/g(x))-1),
					x=(1-bound[intervals]+2^(-53))..(1-bound[intervals-1])):

polyQuickExact[intervals] := truncPoly(polyAccuExact[intervals],polyQuickDegreeHighest):
polyQuick[intervals] := poly_exact2(polyQuickExact[intervals],polyQuickDDCoeffsHighest):

epsQuick[intervals] := numapprox[infnorm](((unapply(polyQuick[intervals],x)(x)/g(x))-1),
					x=(1-bound[intervals]+2^(-53))..(1-bound[intervals-1])):

printf("Checking if the polynomial for interval %d is exactly -1 in z = %f...\n",intervals,1-bound[intervals]);

if (unapply(polyAccurate[intervals],x)(1-bound[intervals]) = -1) then
	printf("  Check passed!\n"):
else
	printf("  Check failed!\n"):
end if:

end if:

for i from 1 to intervals do
	printf("Relative error for accurate phase polynomial in interval %d ([%f;%f]) is 2^(%f)\n",
       		i,bound[i-1],bound[i],log[2](abs(epsAccurate[i]))):
	printf("Relative error for quick phase polynomial in interval %d ([%f;%f]) is 2^(%f)\n",
       		i,bound[i-1],bound[i],log[2](abs(epsQuick[i]))):
od:
printf("Relative error for accurate phase polynomial #1 in special interval [0;sin(2^(-18))]) is 2^(%f)\n",
       log[2](abs(epsAccurateSpecial))):

printf("Relative error for quick phase extra case truncated polynomial #1 in special interval [0;%f]) is 2^(%f)\n",
       extrabound,log[2](abs(epsQuickExtra))):

(PiHalfH, PiHalfM, PiHalfL) := hi_mi_lo(evalf(Pi/2)):

epsPiDD := evalf(((PiHalfH + PiHalfM) - Pi/2)/(Pi/2)):
epsPiTD := evalf(((PiHalfH + PiHalfM + PiHalfL) - Pi/2) / (Pi/2)):

printf("Relative error for storing Pi/2 as a double-double is 2^(%f)\n",evalf(log[2](abs(epsPiDD)))):
printf("Relative error for storing Pi/2 as a triple-double is 2^(%f)\n",evalf(log[2](abs(epsPiTD)))):

# Ce qui suit est pifometrique et doit etre prouve en Gappa ensuite

arithmeticalErrorQuick[1] := 2^(-61):
arithmeticalErrorQuick[2] := 2^(-61):
arithmeticalErrorQuick[3] := 2^(-61):
arithmeticalErrorQuick[4] := 2^(-61):
arithmeticalErrorQuick[5] := 2^(-61):
arithmeticalErrorQuick[6] := 2^(-61):
arithmeticalErrorQuick[7] := 2^(-61):
arithmeticalErrorQuick[8] := 2^(-61):
arithmeticalErrorQuick[9] := 2^(-61):
arithmeticalErrorQuick[10] := 2^(-68):

arithmeticalErrorQuickExtra := 2^(-80):

for i from 1 to intervals do
	estimatedOverallEpsQuick[i] := abs(epsQuick[i]) +
				       abs(arithmeticalErrorQuick[i]) +
                                       abs(epsQuick[i]) * abs(arithmeticalErrorQuick[i]):
	printf("Relative quick phase overall error bound to show in Gappa for interval %d ([%f;%f]) is 2^(%f)\n",
		i,bound[i-1],bound[i],log[2](abs(estimatedOverallEpsQuick[i]))):
od:

estimatedOverallEpsQuickExtra := abs(epsQuickExtra) +
				 abs(arithmeticalErrorQuickExtra) +
                                 abs(epsQuickExtra) * abs(arithmeticalErrorQuickExtra):
printf("Relative quick phase overall error bound to show for extra truncted poly in interval [0;%f]) is 2^(%f)\n",
	extrabound,log[2](abs(estimatedOverallEpsQuickExtra))):

printf("Write tables...\n"):

filename:="TEMPASIN/asin-td.h":
fd:=fopen(filename, WRITE, TEXT):

fprintf(fd, "#include \"crlibm.h\"\n#include \"crlibm_private.h\"\n"):

fprintf(fd, "\n/* File generated by maple/asin-td.mpl */\n\n"):

printf("Write table with bounds\n"):

fprintf(fd, "/* High order words of interval bounds (low order word is 0) */\n"):
for i from 1 to (intervals-1) do
	heb := ieeehexa(bound[i]):
	fprintf(fd, "\#define BOUND%d 0x%s\n",i,heb[1]):
od:

heb := ieeehexa(extrabound):
fprintf(fd, "\#define EXTRABOUND 0x%s\n",heb[1]):

printf("Write additional constants\n"):

fprintf(fd, "\n\n/* Pi/2 as a triple-double*/\n"):
fprintf(fd, "\#define PIHALFH %1.50e\n",PiHalfH):
fprintf(fd, "\#define PIHALFM %1.50e\n",PiHalfM):
fprintf(fd, "\#define PIHALFL %1.50e\n",PiHalfL):

printf("Write table with midpoints and polynomial coefficients\n"):
k := 0:
for i from 0 to polyAccurateDegreeLowest do
	(hi,mi,lo) := hi_mi_lo(coeff(polyAccurate[1],x,i)):
	if ((abs(hi) = 1.0) and (mi = 0) and (lo = 0)) then
		printf(
		"Coefficient %d of interval 1 polynomial is exactly %f and will not be stored in the table\n",i,hi):
	else
	g := 0;
	if (hi <> 0) then
		if (i <= polyQuickDegreeLowest) then
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyLowC%dh, quickPolyLowC%dh */",hi,k,i,i):
		else
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyLowC%dh */",hi,k,i):
		end if:
		k := k + 1:
	end if:
	if (mi <> 0) then
		if (i <= polyQuickDDCoeffsLowest) then
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyLowC%dm, quickPolyLowC%dl */",mi,k,i,i):
		else
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyLowC%dm */",mi,k,i):
		end if:
		k := k + 1:
	end if:
	if (lo <> 0) then
		tbl[k] := sprintf("%1.50e, \t/* %d, accPolyLowC%dl */",lo,k,i):
		k := k + 1:
	end if:
	end if:
od:
tbl[k] := sprintf("%1.50e, \t/* %d, RN rounding constant quick poly low*/",
		compute_rn_constant(estimatedOverallEpsQuick[1]),k):
k := k + 1:
tbl[k] := sprintf("%1.50e, \t/* %d, RD rounding constant quick poly low*/",
		estimatedOverallEpsQuick[1],k):
k := k + 1:
printf("Table for interval 1 written\n"):
for l from 2 to (intervals-1) do
	tbl[k] := sprintf("%1.50e, \t/* %d, midpoint in interval %d*/",midpointFloat[l],k,l):
	tblidx[l] := k;
	k := k + 1;
	for i from 0 to polyAccurateDegreeMiddle do
		(hi,mi,lo) := hi_mi_lo(coeff(polyAccurate[l],x,i)):
		if ((abs(hi) = 1.0) and (mi = 0) and (lo = 0)) then
			printf(
		"Coefficient %d of interval %d polynomial is exactly %f and will not be stored in the table\n",i,l,hi):
		else
		g := 0;
		if (hi <> 0) then
			if (i <= polyQuickDegreeMiddle) then
				tbl[k] := sprintf(
					"%1.50e, \t/* %d, accPolyMid%dC%dh, quickPolyMid%dC%dh */",hi,k,l,i,l,i):
			else
				tbl[k] := sprintf("%1.50e, \t/* %d, accPolyMid%dC%dh */",hi,k,l,i):
			end if:
			k := k + 1:
		end if:
		if (mi <> 0) then
			if (i <= polyQuickDDCoeffsMiddle) then
				tbl[k] := sprintf(
					"%1.50e, \t/* %d, accPolyMid%dC%dm, quickPolyMid%dC%dl */",mi,k,l,i,l,i):
			else
				tbl[k] := sprintf("%1.50e, \t/* %d, accPolyMid%dC%dm */",mi,k,l,i):
			end if:
			k := k + 1:
		end if:
		if (lo <> 0) then
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyMid%dC%dl */",lo,k,l,i):
			k := k + 1:
		end if:
		end if:
	od:
	tbl[k] := sprintf("%1.50e, \t/* %d, RN rounding constant quick poly middle %d*/",
			compute_rn_constant(estimatedOverallEpsQuick[l]),k,l):
	k := k + 1:
	tbl[k] := sprintf("%1.50e, \t/* %d, RD rounding constant quick poly middle %d*/",
			estimatedOverallEpsQuick[l],k,l):
	k := k + 1:
	printf("Table for interval %d written\n",l):
od:
tblidx[intervals] := k:
for i from 0 to polyAccurateDegreeHighest do
	(hi,mi,lo) := hi_mi_lo(coeff(polyAccurate[intervals],x,i)):
	if ((abs(hi) = 1.0) and (mi = 0) and (lo = 0)) then
		printf(
	"Coefficient %d of interval %d polynomial is exactly %f and will not be stored in the table\n",i,intervals,hi):
	else
	g := 0;
	if (hi <> 0) then
		if (i <= polyQuickDegreeHighest) then
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyHighC%dh, quickPolyHighC%dh */",hi,k,i,i):
		else
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyHighC%dh */",hi,k,i):
		end if:
		k := k + 1:
	end if:
	if (mi <> 0) then
		if (i <= polyQuickDDCoeffsHighest) then
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyHighC%dm, quickPolyHighC%dl */",mi,k,i,i):
		else
			tbl[k] := sprintf("%1.50e, \t/* %d, accPolyHighC%dm */",mi,k,i):
		end if:
		k := k + 1:
	end if:
	if (lo <> 0) then
		tbl[k] := sprintf("%1.50e, \t/* %d, accPolyHighC%dl */",lo,k,i):
		k := k + 1:
	end if:
	end if:
od:
tbl[k] := sprintf("%1.50e, \t/* %d, RN rounding constant quick poly high*/",
		compute_rn_constant(estimatedOverallEpsQuick[intervals]),k):
k := k + 1:
tbl[k] := sprintf("%1.50e, \t/* %d, RD rounding constant quick poly high*/",
 		estimatedOverallEpsQuick[intervals],k):
k := k + 1:
printf("Table for interval %d written\n",intervals):
tbllen := k:
printf("The whole table has %d entries, so uses %d bytes of memory\n",tbllen,tbllen*8):
fprintf(fd,"\n\n/* Indices to the following table */\n"):
for i from 2 to intervals do
	fprintf(fd,"\#define TBLIDX%d %d\n",i,tblidx[i]):
od:
fprintf(fd, "\n\n/* Table with midpoints and polynomial coefficients */\n"):
fprintf(fd, "static const double tbl[%d] = {\n",tbllen):
for i from 0 to (tbllen - 1) do
	fprintf(fd, "%s\n",tbl[i]):
od:
fprintf(fd, "};\n\n"):

fclose(fd):

printf("----DONE---\n"):


# Vierachsroboter implizit
# Initialisierung
restart:
kin_constraints_exist := true: # Für Speicherung
;
with(StringTools): # Für Zeitausgabe
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
codegen_act := true:
codegen_opt := 1: # Geringerer Optimierungsgrad. Sonst zu lange.
codegen_debug := 0: # Zur Code-Generierung auch für Nicht-Inert-Ausdrücke
;
read "../helper/proc_MatlabExport":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../robot_codegen_constraints/proc_subs_kintmp_exp":
read "../helper/proc_intersect_circle":
with(RealDomain): # Schränkt alle Funktionen auf den reellen Bereich ein. Muss nach Definition von MatlabExport kommen. Sonst geht dieses nicht.
;
read "../robot_codegen_definitions/robot_env_IC":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name_OL):
# Ergebnisse der Kinematik laden
read sprintf("../codeexport/%s/tmp/kinematics_floatb_%s_rotmat_maple.m", robot_name_OL, base_method_name);
read "../robot_codegen_definitions/robot_env_IC":
Trf := Trf:
Trf_c := Trf_c:
Trf
;
# VGK Gelb 2-7-11-14-6
# Schleife (1-6)-(6-14)
T_1_14 := combine( Matrix(Trf(1..4,1..4, 6)) . Matrix(Trf(1..4,1..4,14))):
# Schleife (1-2)-(2-7)-(7-11)
T_1_11:= combine( Matrix(Trf(1..4,1..4, 2)) . Matrix(Trf(1..4,1..4,7)) . Matrix(Trf(1..4,1..4,11))):
h1t := T_1_14(1..3,4) - T_1_11(1..3,4);

#tmp := Transpose( Matrix(T_1_14(1..3,1..3)) ) . Matrix(T_1_11(1..3,1..3));  nur anzeigen lassen für h1r
;
#combine(tmp);# nur anzeigen lassen für h1r
;
h1r := -(-qJ6(t)+qJ2(t)+qJ7(t)+phi711+qJ11(t))+Pi/2:
# VGK PINK 2-3-12-15-9-8-2
# Schleife (2-3)-(3-12)
T_2_12 := combine(Matrix(Trf(1..4,1..4, 3)) . Matrix(Trf(1..4,1..4,12))):
# Schleife (2-8)-(8-9)-(9-15)
T_2_15:= combine(Matrix(Trf(1..4,1..4, 8)) . Matrix(Trf(1..4,1..4,9)).Matrix(Trf(1..4,1..4, 15))):
h2t := T_2_12(1..3,4) - T_2_15(1..3,4):
tmp := Transpose( Matrix(T_2_12(1..3,1..3)) ) . Matrix(T_2_15(1..3,1..3));  # nur anzeigen lassen für h1r
;
 combine(tmp); # nur anzeigen lassen für h1r
;
h2r:=(qJ3(t)+phi312+qJ12(t)-qJ8(t)-qJ9(t))+3*Pi/2:
# VGK GRÜN 2-3-4-13-16-10-7-2
# Schleife (2-3)-(3-4)-(4-13)
T_2_13 := combine( Matrix(Trf(1..4,1..4, 3)) . Matrix(Trf(1..4,1..4,4)) . Matrix(Trf(1..4,1..4,13))):
# Schleife (2-7)-(7-10)-(10-16)
T_2_16:= combine( Matrix(Trf(1..4,1..4, 7)) . Matrix(Trf(1..4,1..4,10)).Matrix(Trf(1..4,1..4,16)) ):
h3t := T_2_13(1..3,4) - T_2_16(1..3,4):
#tmp := Transpose( Matrix(T_2_13(1..3,1..3)) ) . Matrix(T_2_16(1..3,1..3));  nur anzeigen lassen für h2r
;
#combine(tmp);  nur anzeigen lassen für h3r
;
h3r:=-(qJ3(t)+qJ4(t)+phi413+qJ13(t)-qJ7(t)+phi710-qJ10(t))+5*Pi/2:
# Zusammenstellen aller Zwangsbedingungen
implconstr_t := <h1t([1, 3],1);h2t([1, 2],1);h3t([1, 2],1); h1r; h2r; h3r>; # TODO: In h1r, h2r, h3r muss das richtige drinstehen.
implconstr_s := convert_t_s(implconstr_t):
# Exportiere Code für folgende Skripte
kin_constraints_exist:=true:
save implconstr_t, implconstr_s, kin_constraints_exist, sprintf("../codeexport/%s/tmp/kinematic_constraints_implicit_maple.m", robot_name):
# Exportieren des vollständigen Ausdruckes
if codegen_act then
  MatlabExport(implconstr_s, sprintf("../codeexport/%s/tmp/kinconstr_impl_matlab.m", robot_name), 2):
end if:
# Liste mit abhängigen konstanten Kinematikparametern erstellen (wichtig für Matlab-Funktionsgenerierung)
read "../helper/proc_list_constant_expressions";
kc_symbols := Matrix(list_constant_expressions( implconstr_s )):
save kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_implicit_constraints_symbols_list_maple", robot_name):
MatlabExport(Transpose(kc_symbols), sprintf("../codeexport/%s/tmp/kinematic_implicit_constraints_symbols_list_matlab.m", robot_name), 2):


make_example_surfaces := proc()
 global example_surface;
 local P,T,R,dp,a,u,i,j,k;

 dp := (u,v) -> add(u[i] * v[i],i=1..3):
 R := (x) -> [-x[1]/2+sqrt(3)*x[2]/2,-sqrt(3)*x[1]/2-x[2]/2,x[3]]:

 P := table():
 P[0] := `map/pants`(x -> x,`base/pants`):
 for i from 1 to 5 do P[i] := `sprout/pants`(P[i-1],modp(i-1,3)+1); od:
 T := table():
 T[0] := `pants_joiner/pants_tube`(P[0],2,P[1],3):
 T[1] := `pants_joiner/pants_tube`(P[2],1,P[3],2):
 T[2] := `pants_joiner/pants_tube`(P[4],3,P[5],1):
 example_surface[0] := display(
  seq(`rough_plot/pants`(op(X)),X in entries(P)),
  seq(`plot/pants_tube`(op(X)),X in entries(T))
 ):

 P := table():
 T := table():
 P[0] := eval(`base/pants`):
 P[1] := `sprout/pants`(P[0],1):
 T[0] := `pants_joiner/pants_tube`(P[0],2,P[1],3):
 T[1] := `pants_joiner/pants_tube`(P[0],3,P[1],2):
 example_surface[1] := display(
  seq(`rough_plot/pants`(op(X)),X in entries(P)),
  seq(`plot/pants_tube`(op(X)),X in entries(T))
 ):

 P := table(): T := table():
 P[0] := eval(`base/pants`):
 P[1] := `sprout/pants`(P[0],1):
 P[2] := `map/pants`((x) -> x +~ [0,3,0],P[0]):
 P[3] := `map/pants`((x) -> x +~ [0,3,0],P[1]):
 T[0] := `pants_joiner/pants_tube`(P[0],2,P[2],3):
 T[1] := `pants_joiner/pants_tube`(P[1],3,P[3],2):
 T[3] := `pants_joiner/pants_tube`(P[0],3,P[1],2):
 T[4] := `pants_joiner/pants_tube`(P[2],2,P[3],3):
 example_surface[2] := display(
  seq(`rough_plot/pants`(op(X)),X in entries(P)),
  seq(`plot/pants_tube`(op(X)),X in entries(T))
 ):

 P := table(): T := table():
 P[0] := eval(`base/pants`):
 P[1] := `sprout/pants`(P[0],1):
 T[0] := `pants_joiner/pants_tube`(P[0],2,P[0],3):
 T[1] := `pants_joiner/pants_tube`(P[1],2,P[1],3):
 example_surface[3] := display(
  seq(`rough_plot/pants`(op(X)),X in entries(P)),
  seq(`plot/pants_tube`(op(X)),X in entries(T))
 ):

 a := 2:
 u := table():
 P := table():
 T := table():
 u[0] := simplex_embedding([1,0,0,0]):
 u[1] := simplex_embedding([0,1,0,0]):
 u[2] := simplex_embedding([0,0,1,0]):
 u[3] := simplex_embedding([0,0,0,1]):
 for i from 0 to 3 do
  P[i] := table([
   "centre" = a *~ u[i],
   "circle_centres" = [seq(simplify(a *~ u[i] +~ (u[j] -~ dp(u[j],u[i]) *~ u[i]) *~ (sqrt(27/32))),j in {0,1,2,3} minus {i})],
   "top" = (a + 1/2) *~ u[i]
  ]);
 od:
 T[0,1] := `pants_joiner/pants_tube`(P[0],1,P[1],1):
 T[0,2] := `pants_joiner/pants_tube`(P[0],2,P[2],1):
 T[0,3] := `pants_joiner/pants_tube`(P[0],3,P[3],1):
 T[1,2] := `pants_joiner/pants_tube`(P[1],2,P[2],2):
 T[1,3] := `pants_joiner/pants_tube`(P[1],3,P[3],2):
 T[2,3] := `pants_joiner/pants_tube`(P[2],3,P[3],3):
 example_surface[4] := display(
  seq(`rough_plot/pants`(op(X)),X in entries(P)),
  seq(`plot/pants_tube`(op(X)),X in entries(T)),
  axes=none,scaling=constrained
 ):

 P := table():
 T := table():
 a := 2:
 for i in [-1,1] do
  for j in [-1,1] do
   for k in [-1,1] do
    P[i,j,k] := table([
     "centre" = a *~ [i,j,k],
     "circle_centres" = evalf(map(x -> (a *~ [i,j,k] +~ x /~ sqrt(8)),
			    [[-2*i,j,k],[i,-2*j,k],[i,j,-2*k]])),
     "top" = evalf((a + 1/(2*sqrt(3))) *~ [i,j,k])
    ]);
   od:
  od:
 od:
 T[ 0, 1, 1] := `pants_joiner/pants_tube`(P[ 1, 1, 1],1,P[-1, 1, 1],1):
 T[ 0, 1,-1] := `pants_joiner/pants_tube`(P[ 1, 1,-1],1,P[-1, 1,-1],1):
 T[ 0,-1, 1] := `pants_joiner/pants_tube`(P[ 1,-1, 1],1,P[-1,-1, 1],1):
 T[ 0,-1,-1] := `pants_joiner/pants_tube`(P[ 1,-1,-1],1,P[-1,-1,-1],1):
 T[ 1, 0, 1] := `pants_joiner/pants_tube`(P[ 1, 1, 1],2,P[ 1,-1, 1],2):
 T[ 1, 0,-1] := `pants_joiner/pants_tube`(P[ 1, 1,-1],2,P[ 1,-1,-1],2):
 T[-1, 0, 1] := `pants_joiner/pants_tube`(P[-1, 1, 1],2,P[-1,-1, 1],2):
 T[-1, 0,-1] := `pants_joiner/pants_tube`(P[-1, 1,-1],2,P[-1,-1,-1],2):
 T[ 1, 1, 0] := `pants_joiner/pants_tube`(P[ 1, 1, 1],3,P[ 1, 1,-1],3):
 T[ 1,-1, 0] := `pants_joiner/pants_tube`(P[ 1,-1, 1],3,P[ 1,-1,-1],3):
 T[-1, 1, 0] := `pants_joiner/pants_tube`(P[-1, 1, 1],3,P[-1, 1,-1],3):
 T[-1,-1, 0] := `pants_joiner/pants_tube`(P[-1,-1, 1],3,P[-1,-1,-1],3):
 example_surface[5] := display(
  seq(`rough_plot/pants`(op(X)),X in entries(P)),
  seq(`plot/pants_tube`(op(X)),X in entries(T)),
  axes=none,scaling=constrained
 ):

 return eval(example_surface);
end:


make_alternating_five := proc()
 global alternating_five;
 local G,x2,y2,z2,x3,x5,y5,id,o,oo,inv,idm,om,oom,E,ccl,
       t0,t1,i,j,k,l,g,h,w,m,x,u0,u1,u2,u3,u,v,M,theta;

 G := table():

 x2 := [2,1,4,3,5];
 y2 := [3,4,1,2,5];
 z2 := [1,3,2,5,4];
 x3 := [2,3,1,4,5];
 x5 := [3,5,4,2,1];
 y5 := [2,3,4,5,1];

 id := [1,2,3,4,5];
 o  := (u,v) -> [seq(u[v[i]],i=1..5)];
 oo := proc()
  if   nargs = 0 then return(id);
  elif nargs = 1 then return(args[1]);
  elif nargs = 2 then return(o(args));
  else return o(args[1],oo(args[2..-1]));
  fi;
 end:
 inv := proc(g)
  local gi;
  gi := table():
  for i from 1 to 5 do gi[g[i]] := i; od;
  return [seq(gi[i],i=1..5)];
 end:

 G["id"] := id;
 G["o"]  := eval(o);
 G["oo"]  := eval(oo);
 G["inv"] := eval(inv);

 G["x2"] := x2;  G["x2c"] := convert(x2,disjcyc);
 G["y2"] := y2;  G["y2c"] := convert(y2,disjcyc);
 G["z2"] := z2;  G["z2c"] := convert(z2,disjcyc);
 G["x3"] := x3;  G["x3c"] := convert(x3,disjcyc);
 G["x5"] := x5;  G["x5c"] := convert(x5,disjcyc);
 G["y5"] := y5;  G["y5c"] := convert(y5,disjcyc);

 G["H"] := table():

 G["H"][ 1] := [id];
 G["H"][ 2] := [id,x2];
 G["H"][ 3] := [id,x3,o(x3,x3)];
 G["H"][ 4] := [id,x2,y2,o(x2,y2)];
 G["H"][ 5] := [id,x5,o(x5,x5),oo(x5 $ 3),oo(x5 $ 4)];
 G["H"][ 6] := [seq(seq(o(u,v),u in [id,z2]),v in G["H"][3])];
 G["H"][10] := [seq(seq(o(u,v),u in G["H"][ 2]),v in G["H"][5])];
 G["H"][12] := [seq(seq(o(u,v),u in G["H"][ 4]),v in G["H"][3])];
 G["H"][60] := [seq(seq(o(u,v),u in G["H"][12]),v in G["H"][5])];

 E := G["H"][60];
 G["elements"] := E;

 G["element_index"] := table();
 for i from 1 to 60 do
  G["element_index"][E[i]] := i;
 od:

 G["gens_rule"] := {X2 = x2, Y2 = y2, Z2 = z2, X3 = x3, X5 = x5, Y5 = y5};

 # words in x2, y2, z2 for all elements
 G["words_a"] := [
 [],[X2],[Y2],[X2,Y2],[X2,Z2,Y2,Z2,X2,Z2],[Z2,Y2,Z2,X2,Z2],[Z2,Y2,Z2,X2,Z2,X2],
 [Y2,Z2,Y2,Z2,X2,Z2],[Y2,Z2,X2,Z2,Y2,Z2],[Z2,X2,Z2,Y2,Z2,Y2],[Z2,X2,Z2,Y2,Z2],
 [X2,Z2,X2,Z2,Y2,Z2],[X2,Y2,Z2,Y2],[Y2,Z2,Y2],[X2,Z2,Y2],[Z2,Y2],[X2,Y2,Z2,Y2,Z2,X2,Y2],
 [Y2,Z2,Y2,Z2,X2,Y2],[X2,Z2,Y2,Z2,X2,Y2],[Z2,Y2,Z2,X2,Y2],[X2,Y2,Z2,X2,Z2],[Y2,Z2,X2,Z2],
 [X2,Z2,X2,Z2],[Z2,X2,Z2],[X2,Y2,Z2,X2,Z2,Y2],[Y2,Z2,X2,Z2,Y2],[X2,Z2,X2,Z2,Y2],
 [Z2,X2,Z2,Y2],[X2,Y2,Z2],[Y2,Z2],[X2,Z2],[Z2],[X2,Y2,Z2,Y2,Z2,X2],[Y2,Z2,Y2,Z2,X2],
 [X2,Z2,Y2,Z2,X2],[Z2,Y2,Z2,X2],[Z2,Y2,Z2,Y2,Z2,X2,Z2],[X2,Z2,Y2,Z2,Y2,Z2,X2,Z2],
 [Z2,X2,Z2,X2,Y2],[X2,Z2,X2,Z2,X2,Y2],[Z2,X2],[X2,Z2,X2],[Y2,Z2,X2],[X2,Y2,Z2,X2],
 [X2,Z2,Y2,Z2],[Z2,Y2,Z2],[X2,Y2,Z2,Y2,Z2],[Y2,Z2,Y2,Z2],[Y2,Z2,X2,Y2],[Z2,X2,Y2,Z2],
 [Z2,X2,Y2],[X2,Z2,X2,Y2],[Z2,Y2,Z2,Y2],[X2,Z2,Y2,Z2,Y2],[Y2,Z2,Y2,Z2,Y2],
 [X2,Y2,Z2,Y2,Z2,Y2],[X2,Z2,X2,Z2,X2],[Z2,X2,Z2,X2],[X2,Y2,Z2,X2,Z2,X2],[Y2,Z2,X2,Z2,X2]];

 # words in x2, y2, x3, x5 for all elements
 G["words_b"] := [seq(seq(seq(seq([X2$i,Y2$j,X3$k,X5$l],i=0..1),j=0..1),k=0..2),l=0..4)];

 G["rels"] := [
  [X2$2],[Y2$2],[Z2$2],[X3$3],[X5$5],[Y5$5],
  map(op,[[X2,Y2]$2]),map(op,[[X2,Y2,Z2]$3]),
  map(op,[[Z2,X2,Z2,Y2]$3]),map(op,[[X2,Z2]$5]),map(op,[[Y2,Z2]$5]),
  [X3,Y2,Z2,X2,Z2,Y2,Z2],[Y5,Y2,Z2,X2,Z2,X2],[X5,Y2,X5,X5,X5,X3,X3]];

 ccl := (x) -> sort([op({seq(oo(g,x,inv(g)), g in E)})]);

 G["conjugacy_classes"] := [
  ccl(id),
  ccl(x2),
  ccl(x3),
  ccl(o(x5,x5)),
  ccl(x5)
 ];

 G["conjugacy_class_index"] := table();
 for i from 1 to 5 do
  for g in G["conjugacy_classes"][i] do
   G["conjugacy_class_index"][g] := i;
  od;
 od;

 G["element_order"] := table();

 for g in E do
  h := g;
  i := 1;
  while h <> id do i := i + 1; h := o(g,h); od:
  G["element_order"][g] := i;
 od:

 t0 := (sqrt(5)+1)/2;
 t1 := (sqrt(5)-1)/2;

 # Dodecahedron representation

 idm := [[1,0,0],[0,1,0],[0,0,1]];
 om := (u,v) -> expand(convert(Matrix(u) . Matrix(v),listlist));
 oom := proc()
  if   nargs = 0 then return(idm);
  elif nargs = 1 then return(args[1]);
  elif nargs = 2 then return(om(args));
  else return om(args[1],oom(args[2..-1]));
  fi;
 end:

 G["idm"] := idm;
 G["om"]  := eval(om);
 G["oom"] := eval(oom);

 G["matrix_gens_rule"] := {
   X1 = [[ 1,0,0],[0, 1,0],[0,0, 1]],
   X2 = [[ 1,0,0],[0,-1,0],[0,0,-1]],
   Y2 = [[-1,0,0],[0,-1,0],[0,0, 1]],
   Z2 = [[t1,-t0,-1],[-t0,-1,t1],[-1,t1,-t0]] /~ 2,
   X3 = [[0,0,1],[1,0,0],[0,1,0]],
   X5 = [[t1,-t0,1],[t0,1,t1],[-1,t1,t0]] /~ 2,
   Y5 = [[t0,1,-t1],[-1,t1,-t0],[-t1,t0,1]] /~ 2
 };

 G["matrix"] := table();
 G["axis"]   := table();
 G["angle"]  := table();

 for i from 1 to 60 do
  g := E[i];
  G["matrix"][g] := oom(op(subs(G["matrix_gens_rule"],G["words_b"][i])));
  if i = 1 then
   G["axis"][g] := [0,0,0];
   G["angle"][g] := 0;
  else
   M := Matrix(G["matrix"][g]);
   u0 := convert(NullSpace(M - IdentityMatrix(3))[1],list);
   u0 := simplify(expand(rationalize(u0 /~ sqrt(add(u0[j]^2,j=1..3)))));
   u1 := [1,2,3];
   u1 := u1 -~ add(u1[j] * u0[j],j=1..3) *~ u0;
   u1 := simplify(expand(rationalize(u1)));
   u2 := [u0[2] * u1[3] - u0[3] * u1[2],
          u0[3] * u1[1] - u0[1] * u1[3],
          u0[1] * u1[2] - u0[2] * u1[1]];
   u2 := simplify(expand(rationalize(u2)));
   u3 := simplify(expand(rationalize(convert(M . Vector(u1), list))));

   theta := evalf(arctan(add(u3[j] * u2[j],j=1..3),add(u3[j] * u1[j],j=1..3)));
   theta := round(evalf(60 * theta / Pi)) * Pi / 60;

   if evalf(theta) < 0 then u0 := -~ u0; theta := -theta; fi;
   G["axis"][g] := u0;
   G["angle"][g] := theta;
  fi;
 od;

 alternating_five := eval(G);
 return eval(G);
end:

make_alternating_five():

check_alternating_five := proc()
 local A5,f,fm,is_rot,ok,i,j,ij,g,h,gh,gm,hm,ghm0,ghm1;

 A5 := eval(make_alternating_five()):

 f := (u) -> A5["oo"](op(subs(A5["gens_rule"],u)));
 fm := (u) -> A5["oom"](op(subs(A5["matrix_gens_rule"],u)));

 _ASSERT(
  evalb({op(map(f,A5["rels"]))} = {A5["id"]}),
  "permutation relations");

 _ASSERT(
  evalb({op(map(fm,A5["rels"]))} = {A5["idm"]}),
  "matrix relations");

 _ASSERT(
  `and`(seq(evalb(f(A5["words_a"][i]) = A5["elements"][i]),i=1..60)),
  "words_a");

 _ASSERT(
  `and`(seq(evalb(f(A5["words_b"][i]) = A5["elements"][i]),i=1..60)),
  "words_b");

 is_rot := proc(m)
  local M,N;
  M := Matrix(m);
  if (simplify(expand(Determinant(M) - 1)) <> 0) then
   return false;
  fi;

  N := map(simplify,map(expand,Transpose(M) . M - IdentityMatrix(3)));
  if not(Equal(N,Matrix(3,3))) then return false; fi;
  return true;
 end:

 _ASSERT(
  `and`(seq(is_rot(A5["matrix"][g]),g in A5["elements"])),
  "matrices are rotations"
 );

 ok := true;
 for i from 1 to 60 do
  for j from 1 to 60 do
   g := A5["elements"][i];
   h := A5["elements"][j];
   gh := A5["o"](g,h);
   ij := A5["element_index"][gh];
   gm := A5["matrix"][g];
   hm := A5["matrix"][h];
   ghm0 := A5["matrix"][gh];
   ghm1 := A5["om"](gm,hm);
   if ghm0 <> ghm1 then ok := false; break; fi;
  od:
  if not(ok) then break; fi;
 od:

 _ASSERT(ok,"matrix representation is a homomorphism");
end:


######################################################################
# This section is about the suboperad of little 1-cubes where the
# subintervals fill the whole interval.

`is_element/one_cubes_prime` := (A::set) -> proc(f)
 local X,n,i;
 global reason;

 if not(`is_element/cubes`(1)(A)(f)) then
  reason := [convert(procname,string),"f is not a 1-cube for A",f,A,reason];
  return false;
 fi;

 X := map(a -> map(op,f[a]),[op(A)]);
 X := sort(X,(u,v) -> evalb(u[1]<v[1]));
 n := nops(A);
 if X[1][1] <> 0 then
  reason := [convert(procname,string),"The first interval does not start at 0",X[1]];
  return false;
 fi;
 if X[n][2] <> 1 then
  reason := [convert(procname,string),"The last interval does not end at 1",X[n]];
  return false;
 fi;

 for i from 1 to n-1 do
  if X[i][2] <> X[i+1][1] then
   reason := [convert(procname,string),"Successive intervals do not touch",X[i],X[i+1]];
   return false;
  fi;
 od;

 return true;
end;

`is_equal/one_cubes_prime` := (A::set) -> proc(f1,f2)
 `is_equal/cubes`(1)(A)(f1,f2);
end:

`is_leq/one_cubes_prime` := NULL;

`random_element/one_cubes_prime` := (A::set) -> proc()
 local d,n,u,i,r,R,xy;

 d := 12;
 n := nops(A);
 u := table();
 u[0] := 0;
 for i from 1 to n do
  u[i] := u[i-1] + rand(1..d)();
 od;
 r := u[n];
 for i from 1 to n do u[i] := u[i]/r; od;

 R := `random_element/ord`(A)();
 xy := table();
 for i from 1 to n do
  xy[R[i]] := [[u[i-1]],[u[i]]];
 od;

 return eval(xy);
end;

`list_elements/one_cubes_prime` := NULL;
`count_elements/one_cubes_prime` := NULL;


# Kinematik-Berechnung 3. Arm KAS5
# Beschreibung
#
# Modell m5:
# * Kurbel N7 ist starr mit Körper K2 gekoppelt (über die Achse) (wie m3),
# * Zahnradkopplung zwischen Körpern Z2 und Z3 (Unterschied zu m3)
#
# Setze die Zwangsbedingung (Zahnradkopplung) in die bereits berechneten Zwangsbedingungen für das KAS5m3 ein
#
# Berechnung der Kopplung der Zahnräder am Ellenbogen
# Autor
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-02
# Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Initialisierung
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
kin_constraints_exist := true: # Für Speicherung
;
with(LinearAlgebra):
with(StringTools): # Für Zeitausgabe
;
with(codegen):
with(CodeGeneration):
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
codegen_act := true:
read "../robot_codegen_constraints/proc_subs_kintmp_exp":
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
# Lese Ergebnisse der Kinematik von KAS5m3 aus.
# Siehe KAS5m3_sym_codegen_kinematic_constraints.mw
# kintmp_qs, kintmp_qt, lpar_qs, lpar_qt, kintmp_subsexp
read  sprintf("../codeexport/KAS5m5/tmp/kinematic_constraints_maple_inert.m", robot_name):
kintmp_qs_KAS5m5 := kintmp_qs:
kintmp_qt_KAS5m5 := kintmp_qt:
lpar_qs_KAS5m5 := lpar_qs:
lpar_qt_KAS5m5 := lpar_qt:
kintmp_subsexp_KAS5m5 := kintmp_subsexp:
read  sprintf("../codeexport/KAS5m5/tmp/tree_floatb_definitions", robot_name):
kintmp_s_KAS5m5 := kintmp_s:
kintmp_t_KAS5m5 := kintmp_t:
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
kintmp_s := kintmp_s:
kintmp_t := kintmp_t:
kintmp_qs := kintmp_s:
kintmp_qt := kintmp_t:
kintmp_subsexp := Matrix(2*RowDimension(kintmp_s),2):
for i from 1 to RowDimension(kintmp_s) do
  kintmp_subsexp(2*i-1, 1) := sin(kintmp_s(i,1)):
  kintmp_subsexp(2*i,   1) := cos(kintmp_s(i,1)):
  # Initialisierung der rechten Spalte mit gleichen Werten. Später nur Ersetzung, wenn Vorteilhaft.
  kintmp_subsexp(2*i-1, 2) := kintmp_subsexp(2*i-1, 1):
  kintmp_subsexp(2*i,   2) := kintmp_subsexp(2*i,   1):
end do:
printf("Beginn der Berechnungen. %s\n", FormatTime("%Y-%m-%d %H:%M:%S")):
st := time():
# Übernehme die Ersetzungsausdrücke von KAS5m5
#
for i from 1 to RowDimension(kintmp_s_KAS5m5) do
  for j from 1 to RowDimension(kintmp_s) do
    if kintmp_s_KAS5m5(i,1) = kintmp_s(j,1) then
      # printf("KAS5m5 %d = KAS5m7 %d\n", i, j):
      kintmp_qs(j,1) := kintmp_qs_KAS5m5(i,1):
      kintmp_qt(j,1) := kintmp_qt_KAS5m5(i,1):
    end if:
  end do:
end do:
for i from 1 to RowDimension(kintmp_subsexp_KAS5m5) do
  for j from 1 to RowDimension(kintmp_subsexp) do
    if kintmp_subsexp_KAS5m5(i,1) = kintmp_subsexp(j,1) then
      # printf("KAS5m5 %d = KAS5m7 %d\n", i, j):
      kintmp_subsexp(j,2) := kintmp_subsexp_KAS5m5(i,2):
    end if:
  end do:
end do:
# Hierbei werden die Definitionen für kintmp_s und kintmp_t überschrieben, da die Elemente in kintmp_qs und kintmp_qt drin stehen.
# Umgehung des Problems: Definitionen neu laden.
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
kintmp_s := kintmp_s:
kintmp_t := kintmp_t:
interface(rtablesize=100):
# Korrigiere die Bedingungen. Dort wurden die konstanten Winkel auch mit substitutierten Variablen "s" bezeichnet, was eigentlich keinen Sinn ergibt.
# Die mit Suffix "s" gekennzeichneten substituierten Winkel beziehen sich nur auf Zeitableitungen, die mit einem konstanten Ausdruck ersetzt wurden.
# Damit nicht delta8 und delta8s als unterschiedliche Variablen auftauchen, werden diese ersetzt.
subs_kp1 := <delta8s; delta9s; delta10s; delta12s; delta17s; delta18s>:
subs_kp2 := <delta8;  delta9;  delta10;  delta12;  delta17;  delta18>:
for i from 1 to RowDimension(subs_kp1) do
  for j from 1 to RowDimension(kintmp_subsexp) do
    kintmp_subsexp(j,2) := subs({subs_kp1(i,1)=subs_kp2(i,1)}, kintmp_subsexp(j,2));
  end do:
  for j from 1 to RowDimension(kintmp_qs) do
    kintmp_qs(j,1) := subs({subs_kp1(i,1)=subs_kp2(i,1)}, kintmp_qs(j,1));
    kintmp_qt(j,1) := subs({subs_kp1(i,1)=subs_kp2(i,1)}, kintmp_qt(j,1));
  end do:
  lpar_qs_KAS5m5 := subs({subs_kp1(i,1)=subs_kp2(i,1)}, lpar_qs_KAS5m5);
end do:
# Zahnrad Abwälzbedingung (delta19)
# Siehe Aufzeichnungen vom 3.6.2016
kintmp_s(24);
kintmp_subsexp(47,2);
kintmp_subsexp(48,2);
delta19_qs := convert_t_s(theta(4)):
kintmp_qs(24) := delta19_qs:
kintmp_subsexp(47,2) := sin(delta19_qs):
kintmp_subsexp(48,2) := cos(delta19_qs):
# Zahnrad Abwälzbedingung (rho6)
# Siehe Aufzeichnungen vom 3.6.2016
kintmp_s(28);
kintmp_subsexp(55,2);
kintmp_subsexp(56,2);
rho6_qs := convert_t_s(theta(5)) - convert_t_s(theta(4)) + delta18:
kintmp_qs(28) := rho6_qs:
kintmp_subsexp(55,2) := sin(rho6_qs):
kintmp_subsexp(56,2) := cos(rho6_qs):
# Berechne delta7
# Gl. (30)
kintmp_s(12);
kintmp_subsexp(23,2);
kintmp_subsexp(24,2);
delta7_qs := delta17 + delta19_qs: # Geändert ggü. KAS5
;
kintmp_qs(12) := delta7_qs:
kintmp_subsexp(23,2) := sin(delta7_qs):
kintmp_subsexp(24,2) := cos(delta7_qs):

# Dummy-Einträge für Schnitt-Gelenke
# Die Winkel der Schnittgelenke sind egal, daher keine Berechnung.

# delta 14
#
kintmp_s(19);
kintmp_subsexp(37,2);
kintmp_subsexp(38,2);
kintmp_qs(19) := 0:
# delta 21
#
kintmp_s(26);
kintmp_subsexp(51,2);
kintmp_subsexp(52,2);
kintmp_qs(26) := 0:

# Setze Federlänge als Variable
kintmp_s(29);
kintmp_qs(29) := lpar_qs_KAS5m5:
# Nachverarbeiten
#
kintmp_qt := convert_s_t(kintmp_qs):
# Speichern der Ergebnisse
save kin_constraints_exist, kintmp_qs, kintmp_qt, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert", robot_name):
save kin_constraints_exist, kintmp_qs, kintmp_qt, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
# Exportieren der Zwangsbedingungen
# Matlab-Funktionen für ZB
if codegen_act then
  MatlabExport(kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematik_parallel_kintmp_subsexp_matlab_opt1.m", robot_name), 1):
end if:
printf("Ausdrücke mit Inert-Arctan exportiert (Matlab). %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Liste der Parameter speichern
# Liste mit abhängigen konstanten Kinematikparametern erstellen (wichtig für Matlab-Funktionsgenerierung)
read "../helper/proc_list_constant_expressions";
kc_symbols := Matrix(list_constant_expressions( kintmp_subsexp(..,2) )):
save kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_maple", robot_name):
MatlabExport(Transpose(kc_symbols), sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_matlab.m", robot_name), 2):
printf("Zwangsbedingungen der Parallelstruktur von KAS5m5 nach KAS5m7 angepasst. %s\n", FormatTime("%Y-%m-%d %H:%M:%S")):


# Berechnung und Projektion der Dynamikgleichungen in Regressorform
# Einleitung
# Berechnung und Projektion der Dynamikgleichungen in Regressorform
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# para -> Berechnung für einen parallelen Roboter
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# projection -> Die Dynamikgleichungen werden auf EE-Koordinaten projiziert
# dynamics -> Berechnung der Dynamik
# regressor -> Regressorform (parameterlinear)
#
# TODO
# Die Regressorform ist noch nicht in Minimaldarstellung. Außerdem treten Dynamikparameter auf, die bei reduzierten EE-FG (z.B. 2T1R-Bewegung) keinen Einfluss haben.
#
# Autor
# Tim Job (Studienarbeit bei Moritz Schappler), 2018-12
# Moritz Schappler, moritz.schappler@imes.uni-hannover.de
# (C) Institut für Mechatronische Systeme, Universität Hannover
# Sources
# [GautierKhalil1990] Direct Calculation of Minimum Set of Inertial Parameters of Serial Robots
# [KhalilDombre2002] Modeling, Identification and Control of Robots
# [Ortmaier2014] Vorlesungsskript Robotik I
# [Job2018_S759] Job, T. (Studienarbeit; Betreuer Moritz Schappler): Implementierung einer strukturunabhängigen Dynamikmodellierung für parallelkinematische Maschinen (2018)
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
interface(rtablesize=100):
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
with(StringTools): # Für Zeitausgabe
;
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
#codegen_act := true: # noch nicht implementiert
codegen_opt := 2:
codeexport_invdyn := true:
codeexport_actcoord := false: # Generierung der Dynamik in Antriebskoordinaten nicht standardmäßig (hoher Rechenaufwand)
;
read "../helper/proc_MatlabExport":
# Roboter-Definitionen laden
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", leg_name):
read "../robot_codegen_definitions/robot_env_par":
# Ergebnisse der Plattform-Dynamik in Regressorform laden

dynamicsfile := sprintf("../codeexport/%s/tmp/floatb_%s_platform_dynamic_reg_maple.m", robot_name, base_method_name):
if FileTools[Exists](dynamicsfile) then
  read dynamicsfile:
else
  printf("%s. Plattform-Dynamik (Regressor) konnte nicht geladen werden. Abbruch der Berechnung.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  quit: # Funktioniert in GUI nicht richtig...
  robot_name := "": # ...Daher auch Löschung des Roboternamens.
end if:
# Neu-Definieren, damit Variablen im Workspace auftauchen
paramVecP := paramVecP:
paramVecP_M := paramVecP_M:
A_E := A_E:
H := H:
dH := dH:
M_regmin := M_regmin:
c_regmin := c_regmin:
g_regmin := g_regmin:
# Ergebnisse der Dynamik der Gelenkkette in Regressorform laden

dynamicsfile := sprintf("../codeexport/%s/tmp/invdyn_%s_%s_maple.m", leg_name, "fixb", "regressor_minpar"):
if FileTools[Exists](dynamicsfile) then
  read dynamicsfile:
else
  printf("%s. PKM-Dynamik konnte nicht geladen werden. Abbruch der Berechnung.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  quit: # Funktioniert in GUI nicht richtig...
  robot_name := "": # ...Daher auch Löschung des Roboternamens.
end if:
tau_regressor_s := tau_regressor_s:
tauMM_regressor_s := tauMM_regressor_s:
MMjj_regressor_s := MMjj_regressor_s:
tauC_regressor_s := tauC_regressor_s:
taug_regressor_s := taug_regressor_s:
read "../robot_codegen_definitions/robot_env_par":

# Ergebnisse des Minimalparametervektors der Gelenkkette laden
read sprintf("../codeexport/%s/tmp/minimal_parameter_vector_fixb_maple", leg_name):
Paramvec2 := Paramvec2:
read "../robot_codegen_definitions/robot_env_par": # Nochmal laden, um Standard-Einstellungen überschreiben zu können.
;
# Ergebnisse der zusätzlichen Definitionen für parallele Roboter laden
read sprintf("../codeexport/%s/tmp/para_definitions", robot_name):
printf("Generiere Parameterlineare Form der Dynamik für PKM %s mit Minimal-Parametersatz\n", robot_name):
# Lade "robotics_repo_path"-File mit Link zum "imes-robotics-matlab"-Repo
read("../robotics_repo_path"):
# Lade die Funktionen aus dem "imes-robotics-matlab"-Repo
read(sprintf("%s/transformation/maple/proc_eul%s2r", robotics_repo_path, angleConvLeg)):
read(sprintf("%s/transformation/maple/proc_eul%sjac", robotics_repo_path, "zyx")):

# Berechne Dynamik-Matrizen für alle Beine
# Alle Basisgeschwindigkeiten und -winkel aus Berechnung der seriellen Kette zu null setzen.
omegaxs_base := 0:
omegays_base := 0:
omegazs_base := 0:
alphaxs_base := 0:
betays_base := 0:
gammazs_base := 0:
vxs_base := 0:
vys_base := 0:
vzs_base := 0:
alphaDx_base := 0:
betaDy_base := 0:
gammaDz_base := 0:
vDxs_base := 0:
vDys_base := 0:
vDzs_base := 0:
alphaDDx_base := 0:
betaDDy_base := 0:
gammaDDz_base := 0:

# Ergebnisse der Kinematik für parallelen Roboter laden

kinematicsfile := sprintf("../codeexport/%s/tmp/kinematics_%s_platform_maple.m", robot_name, base_method_name):
if FileTools[Exists](kinematicsfile) then
  read kinematicsfile:
else
  printf("%s. PKM-Kinematik konnte nicht geladen werden. Abbruch der Berechnung.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  quit: # Funktioniert in GUI nicht richtig...
  robot_name := "": # ...Daher auch Löschung des Roboternamens.
end if:
# Variablen neu laden
pivotMat := pivotMat:
pivotMatMas := pivotMatMas:
P_i := P_i:
Jinv := Jinv:
JB_i := JB_i:
JBD_i := JBD_i:
JBinv_i := JBinv_i:
JBDinv_i := JBDinv_i:
U_i := U_i:
UD_i := UD_i:

# Dynamik-Parameter für virtuelle Segmente nach den Plattform-Koppelgelenken entfernen
NQ := NQ - (NQJ-NQJ_parallel):
Paramvec3:=Paramvec2: # Dynamik-Minimalparametervektor der Beinkette
;
for i from NQJ_parallel+1 to NQJ do
  Paramvec3 := subs({XXC||i = 0}, Paramvec3):
  Paramvec3 := subs({XYC||i = 0}, Paramvec3):
  Paramvec3 := subs({XZC||i = 0}, Paramvec3):
  Paramvec3 := subs({YYC||i = 0}, Paramvec3):
  Paramvec3 := subs({YZC||i = 0}, Paramvec3):
  Paramvec3 := subs({ZZC||i = 0}, Paramvec3):
  Paramvec3 := subs({XX||i  = 0}, Paramvec3):
  Paramvec3 := subs({XY||i  = 0}, Paramvec3):
  Paramvec3 := subs({XZ||i  = 0}, Paramvec3):
  Paramvec3 := subs({YY||i  = 0}, Paramvec3):
  Paramvec3 := subs({YZ||i  = 0}, Paramvec3):
  Paramvec3 := subs({ZZ||i  = 0}, Paramvec3):
  Paramvec3 := subs({SX||i  = 0}, Paramvec3):
  Paramvec3 := subs({SY||i  = 0}, Paramvec3):
  Paramvec3 := subs({SZ||i  = 0}, Paramvec3):
  Paramvec3 := subs({MX||i  = 0}, Paramvec3):
  Paramvec3 := subs({MY||i  = 0}, Paramvec3):
  Paramvec3 := subs({MZ||i  = 0}, Paramvec3):
  Paramvec3 := subs({M||i   = 0}, Paramvec3):
end do:
Paramvec2: # Bein-Dynamik-Parametervektor vor Vereinfachungen
;
Paramvec3: # Bein-Dynamik-Parametervektor nach Vereinfachungen
;
paramVecP: # Plattform-Dynamik-Parametervektor ohne Zusammenfassungen
;
paramVecP_M: # Plattform-Dynamik-Parametervektor mit Zusammenfassung mit Bein-Dynamikparametern
;
# Alle Einträge des Minimalparametervektors, die jetzt Null sind, entfernen
ParamvecNew := Matrix(RowDimension(Paramvec3), 1): # Initialisierung mit maximaler Größe
k := 0:
for j from 1 to RowDimension(Paramvec3) do
  if not Paramvec3(j,1) = 0 then
    k := k + 1:
    ParamvecNew(k,1) := Paramvec3(j,1):
  end if:
end do:
# Neuen Parametervektor für die Beinsegmenten (ohne Plattform)
ParamvecNew := ParamvecNew(1..k,1):
# Dupliziere alle berechneten Matrizen. i steht für den Index des jeweiligen Beines
tauReg := tau_regressor_s(7..NQ,..):
#MatrixEnd := index_symmat2vec(NQJ_parallel,NQJ_parallel,NQJ_parallel):
#MMjjReg := MMjj_regressor_s(1..MatrixEnd,..):
tauMMReg := tauMM_regressor_s(7..NQ,..):
for i to NQ do
  tauMMReg := subs({qD_s(i, 1) = 0}, tauMMReg):
end do:
tauCReg := tauC_regressor_s(7..NQ,..):
taugReg := taug_regressor_s(7..NQ,..):

# Regressor aus Berechnung für serielle Beinketten. Dort erste sechs Einträge für Basis
g := <g1;g2;g3>:
tmp := <tmp1;tmp2;tmp3>:
Rmat := Transpose(parse(sprintf("eul%s2r",angleConvLeg))(frame_A_i(1..3,1))):
gtmp := Rmat.g:
for i to NQJ_parallel do
  for k to ColumnDimension(tauReg) do
    for j to 3 do
      tauReg(i,k) := subs({g(j)=tmp(j)},tauReg(i,k)):
      taugReg(i,k) := subs({g(j)=tmp(j)},taugReg(i,k)):
    end do:
    for j to 3 do
      tauReg(i,k) := subs({tmp(j)=gtmp(j)},tauReg(i,k)):
      taugReg(i,k) := subs({tmp(j)=gtmp(j)},taugReg(i,k)):
    end do:
  end do:
end do:

for i to N_LEGS do
  tau_regressor_s||i := Copy(tauReg):
  tauMM_regressor_s||i := Copy(tauMMReg):
  #MMjj_regressor_s||i := Copy(MMjjReg):
  tauC_regressor_s||i := Copy(tauCReg):
  taug_regressor_s||i := Copy(taugReg):
end do:
COLUMNreg := ColumnDimension(tau_regressor_s):
#
# Substituiere in jeder Matrix den Winkel Alpha (Verdrehung in der Basis) und die Gelenkkoordinaten und -geschwindigkeiten
for k from 1 by 1 to N_LEGS do
  for i to NQJ_parallel do
    for j to COLUMNreg do
      for p from NQJ_parallel+1 to NQJ do
        tau_regressor_s||k(i,j) := subs({qJDD||p||s=0,qJD||p||s=0,qJ||p||s=0},tau_regressor_s||k(i,j)):
        tauMM_regressor_s||k(i,j) := subs({qJDD||p||s=0,qJD||p||s=0,qJ||p||s=0},tauMM_regressor_s||k(i,j)):
        tauC_regressor_s||k(i,j) := subs({qJDD||p||s=0,qJD||p||s=0,qJ||p||s=0},tauC_regressor_s||k(i,j)):
        taug_regressor_s||k(i,j) := subs({qJDD||p||s=0,qJD||p||s=0,qJ||p||s=0},taug_regressor_s||k(i,j)):
      end do:
      for l to 3 do
        tau_regressor_s||k(i,j) := subs({frame_A_i(l,1)=frame_A_i(l,k)},tau_regressor_s||k(i,j)):
        tauMM_regressor_s||k(i,j) := subs({frame_A_i(l,1)=frame_A_i(l,k)},tauMM_regressor_s||k(i,j)):
        tauC_regressor_s||k(i,j) := subs({frame_A_i(l,1)=frame_A_i(l,k)},tauC_regressor_s||k(i,j)):
        taug_regressor_s||k(i,j) := subs({frame_A_i(l,1)=frame_A_i(l,k)},taug_regressor_s||k(i,j)):
      end do:
      for m to NQJ_parallel do
        n := (m + (k-1)*NQJ_parallel):
        tau_regressor_s||k(i,j) := subs({qJDD||m||s=qJDD||n||s,qJD||m||s=qJD||n||s,qJ||m||s=qJ||n||s},tau_regressor_s||k(i,j)):
        tauMM_regressor_s||k(i,j) := subs({qJDD||m||s=qJDD||n||s,qJD||m||s=qJD||n||s,qJ||m||s=qJ||n||s},tauMM_regressor_s||k(i,j)):
        tauC_regressor_s||k(i,j) := subs({qJDD||m||s=qJDD||n||s,qJD||m||s=qJD||n||s,qJ||m||s=qJ||n||s},tauC_regressor_s||k(i,j)):
        taug_regressor_s||k(i,j) := subs({qJDD||m||s=qJDD||n||s,qJD||m||s=qJD||n||s,qJ||m||s=qJ||n||s},taug_regressor_s||k(i,j)):
      end do:
    end do:
  end do:
end do:

printf("%s. Dynamik-Matrizen bestimmt. Beginne Projektion/Addition.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
# Berechnung, Projektion und Addition der Dynamikgleichungen
# Berechnung der Kräfte/Momente an den Gelenken der jeweiligen Beine und Projektion auf EE-Plattform
A_E := A_E:
U_i := U_i:
JBinv_i := JBinv_i:
for i to N_LEGS do
   A_||i:= simplify(Transpose(U_i(..,..,i)).Transpose(JBinv_i(..,..,i))).simplify(tau_regressor_s||i):
   Mreg_||i:= simplify(Transpose(U_i(..,..,i)).Transpose(JBinv_i(..,..,i))).simplify(tauMM_regressor_s||i):
   Creg_||i:= simplify(Transpose(U_i(..,..,i)).Transpose(JBinv_i(..,..,i))).simplify(tauC_regressor_s||i):
   greg_||i:= simplify(Transpose(U_i(..,..,i)).Transpose(JBinv_i(..,..,i))).simplify(taug_regressor_s||i):
end do:
# Aufsummieren aller Kräfte, projiziert auf EE-Plattform
Tmp := 0:
TmpM := 0:
TmpC := 0:
Tmpg := 0:
for i to N_LEGS do
  Tmp := Tmp + A_||i:
  TmpM := TmpM + Mreg_||i:
  TmpC := TmpC + Creg_||i:
  Tmpg := Tmpg + greg_||i:
end do:
A_j := Tmp:
Mreg_j := TmpM:
Creg_j := TmpC:
greg_j := Tmpg:
# Addiere Inverse Dynamik der Plattform
ROWS := RowDimension(ParamvecNew):
#
# Neuer Parametervektor für Beine und Plattform
paramMin := <ParamvecNew;paramVecP>: # TODO: Hier stand vorher paramVecP_M. Damit war die Regressorform aber nicht für alle Systeme konsistent. Jetzt ist die parameterlineare Form aber nicht mehr so stark zusammengefasst, wie sie es sein könnte.
;
# Regressormatrix zusammenstellen
A := pivotMat.<A_j(1..6,1..ROWS)|A_E>:
Mreg := pivotMat.<Mreg_j(1..6,1..ROWS)|M_regmin>:
Creg := pivotMat.<Creg_j(1..6,1..ROWS)|c_regmin>:
greg := pivotMat.<greg_j(1..6,1..ROWS)|g_regmin>:
NotNullEntries := 0:
for i to RowDimension(paramMin) do
	if not(paramMin(i,1) = 0 or Equal(Column(A,i), ZeroVector[column](RowDimension(A)))) then
		NotNullEntries := NotNullEntries + 1:
	end if:
end do:
NotNullEntries:
paramMinRed := Matrix(NotNullEntries,1):
ARed := Matrix(RowDimension(A),NotNullEntries):
MregRed := Matrix(RowDimension(A),NotNullEntries):
CregRed := Matrix(RowDimension(A),NotNullEntries):
gregRed := Matrix(RowDimension(A),NotNullEntries):
j := 1:
for i to RowDimension(paramMin) do
	if not(paramMin(i,1) = 0 or Equal(Column(A,i), ZeroVector[column](RowDimension(A)))) then
		paramMinRed(j,1) := paramMin(i,1):
		for k to RowDimension(A) do
			ARed(k,j) := A(k,i):
			MregRed(k,j) := Mreg(k,i):
			CregRed(k,j) := Creg(k,i):
			gregRed(k,j) := greg(k,i):
		end do:
		j := j + 1:
	end if:
end do:
printf("%s. Beginne Ersetzung der Geschwindigkeiten und Bestimmung der Plattform-Terme.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
# Replace Joint Velocities
# Substituiere die Gelenkgeschwindigkeiten über H-, Ui- und JBi-Matrix mit EE-Geschwindikeiten
Tmp := 0:
Tmp2 := 0:

for i to N_LEGS do
  Tmp := Multiply(H,xED_s):
  Tmp := Multiply(U_i(..,..,i),Tmp):
  z||i := simplify(Multiply(JBinv_i(..,..,i),Tmp)):
  Tmp2 := JBinv_i(..,..,i).(Multiply(U_i(..,..,i),H).xEDD_s+Multiply(U_i(..,..,i),dH).xED_s+Multiply(UD_i(..,..,i),H).xED_s):
  A||i := simplify(Multiply(JBinv_i(..,..,i),JBD_i(..,..,i))):
  B||i := Multiply(A||i,Matrix(qJD_i_s(..,i))):
  C||i := Tmp2 - B||i:
end do:

RowParamMin := RowDimension(paramMinRed):
MtoC := copy(MregRed):
for i to NX do
  for k to RowParamMin do
    for j to N_LEGS do
      for l to NQJ_parallel do
        ARed(i,k) := subs({qJDD_i_s(l,j)=C||j(l)},ARed(i,k)):
        MregRed(i,k) := subs({qJDD_i_s(l,j)=C||j(l)},MregRed(i,k)):
        MtoC(i,k) := subs({qJDD_i_s(l,j)=C||j(l)},MtoC(i,k)):
      end do:
    end do:
    for j to N_LEGS do
      for l to NQJ_parallel do
        ARed(i,k) := subs({qJD_i_s(l,j)=z||j(l)},ARed(i,k)):
        MregRed(i,k) := subs({qJD_i_s(l,j)=z||j(l)},MregRed(i,k)):
        MtoC(i,k) := subs({qJD_i_s(l,j)=z||j(l)},MtoC(i,k)):
        CregRed(i,k) := subs({qJD_i_s(l,j)=z||j(l)},CregRed(i,k)):
      end do:
    end do:
    for j to N_LEGS do
      for l to 6 do
        MregRed(i,k) := subs({xED_s(l,1)=0},MregRed(i,k)):
        MtoC(i,k) := subs({xEDD_s(l,1)=0},MtoC(i,k)):
      end do:
    end do:
  end do:
end do:
CregRed := CregRed+MtoC:
tauGes := ARed:#.paramMinRed:
# Dynamik-Regressor in Plattform-Koordinaten (oben berechnet)
tau_x := tauGes:
# Dynamik-Regressor in Antriebs-Koordinaten umrechnen. Nur machen, wenn die Jacobi-Matrix einfach genug ist.
if RowDimension(Jinv) < 5 and codeexport_actcoord then
  read sprintf("../codeexport/%s/tmp/jacobian_maple.m", robot_name): # Annahme, dass die Jacobi-Matrix (aus Invertierung) vorher berechnet wurde. Setzt aktuell das normale Dynamik-Skript voraus.
  J:=J: # Neudefinition, damit Variable im Workspace ist.
  tau_qa:=Transpose(J).tauGes:
  Mreg_qa:=Transpose(J).MregRed:
  Creg_qa:=Transpose(J).CregRed:
  greg_qa:=Transpose(J).gregRed:
end if:

# Erzeuge Minimalkoordinatenvektor zur Erstellung der Massenmatrix durch Ableitungen im folgenden Schritt
xEDD_s_dummy := Matrix(NX,1):
zaehler := 1:
for i to 6 do
	if not(xEDD_s(i) = 0) then
		xEDD_s_dummy(zaehler) := xEDD_s(i):
		zaehler := zaehler + 1:
	end if:
end do:
# Erzeugung der Massenmatrizen
MMregRed := Matrix(NX*NX, RowDimension(paramMinRed)):
MMreg_qa := Matrix(NX*NX, RowDimension(paramMinRed)):
i_rr := 0:
for i to NX do # Zeilenindex der Massenmatrix
  for j to NX do  # Spaltenindex der Massenmatrix
    #if j > i then
    #  next: # rechte obere Seite der symmetrischen Matrix. Keine neue Information. Nicht berechnen oder speichern.
    #end if:
    i_rr := i_rr + 1: # Gehe zeilenweise durch den unteren linken Teil der Massenmatrix (inkl. Diagonale)
    for k to RowDimension(paramMinRed) do # Spaltenindex der Regressormatrix
      MMregRed[i_rr, k] := diff(MregRed[i, k], xEDD_s_dummy[j, 1]):
      MMreg_qa[i_rr, k] := diff(Mreg_qa[i, k], xEDD_s_dummy[j, 1]):
    end do:
  end do:
end do:
# Dynamik in Plattform-Koordinaten (oben berechnet)
tau_x := ARed:
MMreg_x := MMregRed:
Creg_x := CregRed:
greg_x := gregRed:
# PKM-Dynamik-Funktionen mit MPV bereits eingesetzt
# Erzeuge MPV-Vektor
nMDP := RowDimension(paramMinRed):
PV := Matrix(nMDP,1):
for i from 1 to nMDP do
  PV(i,1) := parse(sprintf("MDP%d%", i)):
end do:

tau_x_mdp := tau_x.PV:
MMreg_x_mdp := MMreg_x.PV:
Creg_x_mdp := Creg_x.PV:
greg_x_mdp := greg_x.PV:

tau_qa_mdp := tau_qa.PV:
MMreg_qa_mdp := MMreg_qa.PV:
Creg_qa_mdp := Creg_qa.PV:
greg_qa_mdp := greg_qa.PV:

# Export
# Maple-Export (zur eventuellen späteren Verarbeitung in Maple)

save tau_x,   sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_reg_maple.m",     robot_name):
save MMreg_x, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_MMreg_maple.m",   robot_name):
save Creg_x,  sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_tauCreg_maple.m", robot_name):
save greg_x,  sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_taugreg_maple.m", robot_name):

# Matlab Export: Floating base (TODO: Was heißt das? Hier ist doch kein Floating Base?)
# Berechnung der Basis-Belastung ist für manche Basis-Darstellungen falsch (siehe oben unter Gravitationslast). (TODO: Ist das noch aktuell?)

if codeexport_invdyn then
  printf("%s. Beginne Code-Export Inverse Dynamik (Regressor) in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(tau_x, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_reg_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Massenmatrix (MDP eingesetzt) in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(MMreg_x, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_MMreg_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Coriolis-Vektor (MDP eingesetzt) in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(Creg_x, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_tauCreg_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Gravitations-Vektor (MDP eingesetzt) in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(greg_x, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_taugreg_matlab.m", robot_name), codegen_opt):
end if:

if codeexport_invdyn and RowDimension(Jinv) < 5 and codeexport_actcoord then
  printf("%s. Beginne Code-Export Inverse Dynamik (Regressor) in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(tau_qa, sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_reg_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Massenmatrix (Regressor) in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(MMreg_qa, sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_MMreg_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Coriolis-Vektor (Regressor) in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(Creg_qa, sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_tauCreg_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Gravitations-Vektor (Regressor) in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(greg_qa, sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_taugreg_matlab.m", robot_name), codegen_opt):
end if:

# Maple-Export

save paramMinRed, sprintf("../codeexport/%s/tmp/minimal_parameter_parrob_maple.m", robot_name):
save RowParamMin, sprintf("../codeexport/%s/tmp/RowMinPar_parallel_maple.m",       robot_name):

MatlabExport(paramMinRed, sprintf("../codeexport/%s/tmp/minimal_parameter_parrob_matlab.m", robot_name), codegen_opt):
MatlabExport(RowParamMin, sprintf("../codeexport/%s/tmp/RowMinPar_parallel.m", robot_name), 2);

# PKM-Dynamik-Funktionen mit MPV bereits eingesetzt

# Maple-Export

save tau_x_mdp,   sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_reg_mdp_maple.m",     robot_name):
save MMreg_x_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_MMreg_mdp_maple.m",   robot_name):
save Creg_x_mdp,  sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_tauCreg_mdp_maple.m", robot_name):
save greg_x_mdp,  sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_taugreg_mdp_maple.m", robot_name):

# Matlab Export:
if codeexport_invdyn then
  printf("%s. Beginne Code-Export Inverse Dynamik (MDP eingesetzt) in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(tau_x_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_reg_mdp_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Massenmatrix (MDP eingesetzt) in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(MMreg_x_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_MMreg_mdp_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Coriolis-Vektor (MDP eingesetzt) in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(Creg_x_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_tauCreg_mdp_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Gravitations-Vektor (MDP eingesetzt) in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(greg_x_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_taugreg_mdp_matlab.m", robot_name), codegen_opt):
end if:

if codeexport_invdyn and RowDimension(Jinv) < 5 and codeexport_actcoord then
  printf("%s. Beginne Code-Export Inverse Dynamik (MDP eingesetzt) in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(tau_qa_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_reg_mdp_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Massenmatrix (MDP eingesetzt) in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(MMreg_qa_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_MMreg_mdp_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Coriolis-Vektor (MDP eingesetzt) in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(Creg_qa_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_tauCreg_mdp_matlab.m", robot_name), codegen_opt):
  printf("%s. Beginne Code-Export Gravitations-Vektor (MDP eingesetzt) in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(greg_qa_mdp, sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_taugreg_mdp_matlab.m", robot_name), codegen_opt):
end if:


`is_element/itloc/I` := (n::nonnegint) -> (i) ->
 type(i,nonnegint) and i < n:

`list_elements/itloc/I` := (n::nonnegint) -> [seq(i,i=0..n-1)]:

######################################################################

`is_element/itloc/P` := (n::nonnegint) -> (A) ->
 type(A,set(nonnegint)) and (nops(A) = 0 or max(0,op(A)) < n):

`list_elements/itloc/P` := (n::nonnegint) ->
 [op(combinat[powerset]({seq(i,i=0..n-1)}))];

`is_leq/itloc/P` := (n::nonnegint) -> (A,B) -> evalb (A minus B = {});

`bot/itloc/P` := (n::nonnegint) -> {};

`top/itloc/P` := (n::nonnegint) -> {seq(i,i=0..n-1)};

`sup/itloc/P` := (n::nonnegint) -> (A,B) -> A union B;

`inf/itloc/P` := (n::nonnegint) -> (A,B) -> A intersect B;

`angle/itloc/P` := (n::nonnegint) -> (A,B) ->
 (A = {} or B = {} or max(op(A)) <= min(op(B)));

######################################################################

`is_element/itloc/Q` := (n::nonnegint) -> proc(U)
 local N,A,b;

 if not type(U,set(set(nonnegint))) then
  return false;
 fi;

 if n = 0 then
  return evalb(U minus {{}} = {});
 fi;

 if max(0,op(map(op,U))) >= n then
  return false;
 fi;

 N := `list_elements/itloc/I`(n);

 for A in U do
  for b in N minus A do
   if not(member(A union {b},U)) then
    return false;
   fi;
  od;
 od;

 return true;
end:

`is_leq/itloc/Q` := (n::nonnegint) -> (U,V) -> evalb (V minus U = {});

`itloc/u` := (n::nonnegint) -> proc (A::set(nonnegint))
 local BB;

 BB := `list_elements/itloc`(n) minus A;

 return map(B -> A union B,BB);
end:

`bot/itloc/Q` := (n::nonnegint) -> `itloc/u`(n)({});

`top/itloc/Q` := (n::nonnegint) -> {};

`omul/itloc/Q` := (n::nonnegint) -> proc(U,V)
 local UV;

 UV := {seq(seq([A,B],B in V),A in U)};

 UV := select(AB -> `angle/itloc/P`(n)(op(AB)),UV);

 return UV;
end:

`mul/itloc/Q` := (n::nonnegint) -> (U,V) ->
 map(AB -> AB[1] union AB[2],`omul/itloc/Q`(U,V));

######################################################################

`is_element/itloc/M` := (n::nonnegint) -> (AB) ->
 type(AB,list) and nops(AB) = 2 and
  `is_element/itloc/P`(n)(AB[1]) and
  `is_element/itloc/P`(n)(AB[2]) and
  `itloc/angle`(n)(op(AB));

`list_elements/itloc/M` := proc(n::nonnegint)
 local P,Q,i;

 P := `list_elements/itloc/P`(n);
 Q := table();
 for i from 0 to n do
  Q[i] := combinat[powerset]({seq(j,j=i..n-1)});
 end:
 return [seq(seq([A,B],B in Q[max(0,op(A))]),A in P)];
end:

`is_leq/itloc/M` := (n::nonnegint) -> (AB0,AB1) ->
 evalb (`is_leq/itloc/P`(n)(AB0[1],AB1[1]) and
        `is_leq/itloc/P`(n)(AB0[2],AB1[2]));

`bot/itloc/M` := (n::nonnegint) -> [{},{}];

`inf/itloc/M` := (n::nonnegint) -> (AB0,AB1) ->
 [AB0[1] intersect AB1[1],AB0[2] intersect AB1[2]];

`zeta/itloc`  := (n::nonnegint) -> B -> [{},B];
`xi/itloc`    := (n::nonnegint) -> A -> [A,{}];

`sigma/itloc` := (n::nonnegint) -> AB -> AB[1] union AB[2];

`alpha/itloc` := (n::nonnegint) -> (i) -> (AB) ->
 [select(a -> a < i,AB[1]), select(a -> a >= i,AB[1]) union AB[2]];

`beta/itloc`  := (n::nonnegint) -> (i) -> (AB) ->
 [select(a -> a <= i,AB[1]), select(a -> a >= i,AB[1]) union AB[2]];

`gamma/itloc` := (n::nonnegint) -> (i) -> (AB) ->
 [AB[1] union select(b -> b < i,AB[2]), select(b -> b >= i,AB[2])];

`delta/itloc` := (n::nonnegint) -> (i) -> (AB) ->
 [AB[1] union select(b -> b <= i,AB[2]), select(b -> b >= i,AB[2])];

######################################################################

`is_element/itloc/L` := (n::nonnegint) -> proc(U)
 local N,AB,A,B,a,b,A1,B1,i,j;

 if not type(U,set(list(set(nonnegint)))) then
  return false;
 fi;

 N := `list_elements/itloc/I`(n);

 for AB in U do
  if not(`is_element/itloc/M`(n)(AB)) then
   return false;
  fi;
 od;

 for AB in U do
  A,B = op(AB);
  a := max(0,op(A));
  b := min(n-1,op(B));
  A1 := {seq(i,i=0..b)} minus A;
  B1 := {seq(i,i=a..n-1)} minus B;
  for i in A1 do
   if not(member([A union {i},B]),U) then return false; fi;
  od;
  for j in B1 do
   if not(member([A,B union {j}]),U) then return false; fi;
  od;
 od;

 return true;
end:

`is_leq/itloc/L` := (n::nonnegint) -> (U,V) -> evalb (V minus U = {})

`itloc/v` := (n::nonnegint) -> proc (AB::[set(nonnegint),set(nonnegint)])
 local a,b,A,A1,A2,A3,B,B1,L;

 A,B = op(AB);
 b := min(n-1,op(B));
 A1 := {seq(i,i=0..b)} minus A;

 L := NULL;
 for A2 in combinat[powerset](A1) do
  A3 := A union A2;
  a := max(0,op(A3));
  B1 := {seq(j,j=a..n-1)} minus B;
  L := L,seq([A3,B union B2],B2 in combinat[powerset](B1));
 od:
 return {L};
end:


input := FileTools:-Text:-ReadFile("AoC-2021-8-input.txt" ):

lines := subs(""=NULL,(op@StringTools:-Split)~(StringTools:-Split(input, "\n"),"|"));

# part 1, just count the 1, 4, 7, and 8s
total := 0:
for i from 2 to nops(lines) by 2 do
    tal := table(sparse=0,Statistics:-Tally(map(length, StringTools:-Split(lines[i]))));
    total := total + tal[2] + tal[3] + tal[4] + tal[7];
end do:

answer1 := total;

# part 2

sortstring := s->StringTools:-Join( sort( StringTools:-Explode(s) ), "");
lookups := ["abcefg"="0", "acdeg"="2", "acdfg"="3", "abdfg"="5", "abdefg"="6", "abcdfg"="9"];
alphab := {seq(j,j in "abcdefg")};

Decode := proc( code, output )
local j, displays, cypher, i, decoded, w, wl, l, wc, n;

    displays := sort( StringTools:-Split(StringTools:-Trim(code)," "), length );

    cypher := table([seq(i={seq(j,j in "abcdefg")},i in "abcdefg")]):
    decoded := table();

    for w in displays do
        wl := {seq(i, i in w)};
        if length(w) = 2 and not assigned(decoded["1"]) then # 1
            decoded["1"] := sortstring(w);
            # may be
            cypher["c"] := cypher["c"] intersect wl;
            cypher["f"] := cypher["f"] intersect wl;
            # can't be
            for l in "abdeg" do
                cypher[l] := cypher[l] minus wl;
            end do;
        elif length(w) = 3 and not assigned(decoded["7"])then # 7
            decoded["7"] := sortstring(w);
            cypher["c"] := cypher["c"] intersect {seq(i, i in w)};
            cypher["f"] := cypher["f"] intersect {seq(i, i in w)};
            cypher["a"] := cypher["a"] intersect {seq(i, i in w)};
            if assigned(decoded["2"]) then
                cypher["a"] := cypher["a"] intersect {seq(i, i in w)} minus cypher["c"];
            end if;
            if nops( cypher["a"] ) = 1 then
                for l in alphab minus {"a"} do
                    cypher[l] := cypher[l] minus cypher["a"];
                end do;
            end if;
            for l in "bdeg" do
                cypher[l] := cypher[l] minus wl;
            end do;
        elif length(w) = 4 and not assigned(decoded["4"])then # 4
            decoded["4"] := sortstring(w);
            cypher["b"] := cypher["b"] intersect {seq(i, i in w)};
            cypher["d"] := cypher["d"] intersect {seq(i, i in w)};
            cypher["c"] := cypher["c"] intersect {seq(i, i in w)};
            cypher["f"] := cypher["f"] intersect {seq(i, i in w)};
            for l in "aeg" do
                cypher[l] := cypher[l] minus wl;
            end do;
        elif length(w) = 7 and not assigned(decoded["8"]) then # 8
            decoded["8"] := sortstring(w);
            cypher["b"] := cypher["b"] intersect {seq(i, i in w)};
            cypher["d"] := cypher["d"] intersect {seq(i, i in w)};
            cypher["c"] := cypher["c"] intersect {seq(i, i in w)};
            cypher["f"] := cypher["f"] intersect {seq(i, i in w)};
        elif length(w) = 6 then # 0, 6, 9 missing letters must be c,d,e
            wc := alphab minus wl;
            for l in "abf" do
                cypher[l] := cypher[l] minus wc;
            end do;
        elif length(w) = 5 then # 2, 3, 5 missing letters must be b,c,e,f
            wc := alphab minus wl;
            for l in "adg" do
                cypher[l] := cypher[l] minus wc;
            end do;
        end if;
    end do:

    for l in alphab do
        if nops(cypher[l]) = 1 then
            for n in alphab minus {l} do
                cypher[n] := cypher[n] minus cypher[l];
            end do;
        end if;
    end do;

    for l in alphab do
        if nops(cypher[l]) <> 1 then
            error "need more!";
        end if;
        cypher[l] := op(cypher[l]);
    end do:

    cypher := sort((rhs=lhs)~([entries(cypher,pairs)]));
    decoded := sort((rhs=lhs)~([entries(decoded,pairs)]));

    tmp := map(sortstring, StringTools:-Split(output," "));
    tmp := map2(subs, decoded, tmp);
    tmp := (StringTools:-Join([seq]( subs(lookups, StringTools:-Join(sort( subs(cypher, convert(w,list)) ),"")), w in tmp), "") );

    return parse(tmp);

end proc:

answer2 := add( Decode(lines[i], lines[i+1]), i=1..nops(lines), 2);


apply_linear := proc(f,T := rational)
 return
   proc(u)
    local c,v;

    if u = 0 then
     return 0;
    elif type(u,`+`) or type(u,list) or type(u,set) then
     return map(apply_linear(f,T),u);
    elif type(u,`*`) then
     c,v := selectremove(type,u,T);
     return c * f(v);
    elif type(u,T) then
     return u * f(1);
    else
     return f(u);
    fi;
   end:
end:

apply_linear_mod := (f,m::posint) -> proc(u)
 local c,v;

 if u = 0 then
  return 0;
 elif type(u,`+`) or type(u,list) or type(u,set) then
  return modp(map(apply_linear(f),u),m);
 elif type(u,`*`) then
  c,v := selectremove(type,u,integer);
  c := modp(c,m);
  if c = 0 then
   return 0;
  else
   return modp(expand(c * f(v)),m);
  fi;
 elif type(u,integer) then
  return modp(u * f(1),m);
 else
  return f(u);
 fi;
end:

apply_list_linear := (f) -> proc(u)
 local c,v;

 if u = 0 then
  return 0;
 elif type(u,`+`) or type(u,list) or type(u,set) then
  return map(apply_list_linear(f),u);
 elif type(u,`*`) then
  c,v := selectremove(type,u,rational);
  return c * f(v);
 elif type(u,rational) then
  return u * f(1);
 else
  return f(u);
 fi;
end:

apply_list_linear_mod := (f,m::posint) -> proc(u)
 local c,v;

 if u = 0 then
  return 0;
 elif type(u,`+`) or type(u,list) or type(u,set) then
  return modp(map(apply_list_linear(f),u),m);
 elif type(u,`*`) then
  c,v := selectremove(type,u,integer);
  c := modp(c,m);
  if c = 0 then
   return 0;
  else
   return modp(expand(c * f(v)),m);
  fi;
 elif type(u,rational) then
  return modp(u * f(1),m);
 else
  return f(u);
 fi;
end:

apply_bilinear := (f) -> proc(u,v)
 local cu,xu,cv,xv;

 if u = 0 or v = 0 then
  return 0;
 elif type(u,`+`) then
  return map(apply_bilinear(f),u,v);
 elif type(v,`+`) then
  return map2(apply_bilinear(f),u,v);
 fi;

 if type(u,`*`) then
  cu,xu := selectremove(type,u,rational);
 else
  cu,xu := 1,u;
 fi;

 if type(v,`*`) then
  cv,xv := selectremove(type,v,rational);
 else
  cv,xv := 1,v;
 fi;

 return cu * cv * f(xu,xv);
end:

apply_bilinear_mod := (f,m::posint) -> proc(u,v)
 local cu,xu,cv,xv,c;

 if u = 0 or v = 0 then
  return 0;
 elif type(u,`+`) then
  return modp(map(apply_bilinear(f),u,v),m);
 elif type(v,`+`) then
  return modp(map2(apply_bilinear(f),u,v),m);
 fi;

 if type(u,`*`) then
  cu,xu := selectremove(type,u,integer);
 else
  cu,xu := 1,u;
 fi;

 if type(v,`*`) then
  cv,xv := selectremove(type,v,integer);
 else
  cv,xv := 1,v;
 fi;

 c := modp(cu * cv,m);
 if c = 0 then
  return 0;
 else
  return modp(expand(c * f(xu,xv)),m);
 fi;
end:

apply_assoc := (f,e) -> proc()
 if nargs = 0 then
  return e;
 elif nargs = 1 then
  return args[1];
 elif args = 2 then
  return f(args[1],args[2]);
 else
  return f(args[1],apply_assoc(f,e)(args[2..-1]));
 fi;
end:

apply_linear_assoc := (f,e) -> proc()
 if nargs = 0 then
  return e;
 elif nargs = 1 then
  return args[1];
 elif args = 2 then
  return apply_bilinear(f)(args[1],args[2]);
 else
  return apply_bilinear(f)(args[1],apply_linear_assoc(f,e)(args[2..-1]));
 fi;
end:

apply_linear_assoc_mod := (f,e,m::posint) -> proc()
 if nargs = 0 then
  return e;
 elif nargs = 1 then
  return args[1];
 elif args = 2 then
  return apply_bilinear_mod(f,m)(args[1],args[2]);
 else
  return apply_bilinear_mod(f,m)(args[1],apply_linear_assoc_mod(f,e,m)(args[2..-1]));
 fi;
end:

apply_deg := proc(f,deg_type := rational)
 return
  proc(u)
   local c,v,d,e,i,n;

   if u = 0 then
    return 0;
   elif type(u,`+`) then
    d := map(apply_deg(f),{op(u)});
    if nops(d) = 1 then
     return d[1];
    else
     return FAIL;
    fi;
   elif type(u,`*`) then
    d := map(apply_deg(f),[op(u)]);
    if type(d[1],deg_type) then
     return `+`(op(d));
    elif type(d[1],list) then
     e := d[1];
     for i from 2 to nops(d) do e := e +~ d[i]; od;
     return e;
    else
     return FAIL;
    fi;
   elif type(u,`^`) and type(op(2,u),deg_type) then
    d := apply_deg(f)(op(1,u));
    n := op(2,u);
    if type(d,deg_type) then
     return n*d;
    elif type(d,list) then
     return n *~ d;
    else
     return FAIL;
    fi;
   else
    return f(u);
   fi;
  end:
end:

apply_max_deg := (f) -> proc(u)
 local c,v,d,e,i,n;

 if u = 0 then
  return 0;
 elif type(u,`+`) then
  return max(op(map(apply_max_deg(f),{op(u)})));
 elif type(u,`*`) then
  d := map(apply_max_deg(f),[op(u)]);
  if type(d[1],rational) then
   return `+`(op(d));
  elif type(d[1],list) then
   e := d[1];
   for i from 2 to nops(d) do e := e +~ d[i]; od;
   return e;
  else
   return FAIL;
  fi;
 elif type(u,`^`) and type(op(2,u),rational) then
  d := apply_max_deg(f)(op(1,u));
  n := op(2,u);
  if type(d,rational) then
   return n*d;
  elif type(d,list) then
   return n *~ d;
  else
   return FAIL;
  fi;
 else
  return f(u);
 fi;
end:

apply_leibniz := (f) -> proc(u)
 local n,v,uu,i;

 if type(u,`+`) or type(u,list) or type(u,set) then
  return map(apply_leibniz(f),u);
 elif type(u,`*`) then
  uu := [op(u)];
  return expand(add(`*`(op(subsop(i = apply_leibniz(f)(uu[i]),uu))),i=1..nops(uu)));
 elif type(u,`^`) then
  v,n := op(u);
  return n * v^(n-1) * apply_leibniz(f)(v);
 elif type(u,rational) then
  return 0;
 else
  return f(u);
 fi;
end:

apply_leibniz_mod := (f,m::posint) -> proc(u)
 local n,v,uu,i;

 if type(u,`+`) or type(u,list) or type(u,set) then
  return map(apply_leibniz(f),u);
 elif type(u,`*`) then
  uu := [op(u)];
  return modp(expand(add(`*`(op(subsop(i = apply_leibniz(f)(uu[i]),uu))),i=1..nops(uu))),m);
 elif type(u,`^`) then
  v,n := op(u);
  if modp(n,m) = 0 then
   return 0;
  else
   return modp(expand(n * v^(n-1) * apply_leibniz(f)(v)),m);
  fi;
 elif type(u,rational) then
  return 0;
 else
  return f(u);
 fi;
end:


`is_element/FFbar` := (N::posint) -> (A::set) -> proc(Q)
 local B,C,P,P1;
 global reason;

 P := {op(`list_elements/big_subsets`(A))};

 if not(is_table_on(P)(Q)) then
  reason := [convert(procname,string),"Q is not a table indexed by big subsets of A",eval(Q)];
  return false;
 fi;

 for B in P do
  if not(`is_element/SCP`(N)(B)(Q[B])) then
   reason := [convert(procname,string),"Q[B] is not in SCP(N)(B)",eval(Q[B]),N,B,reason];
   return false;
  fi;

  P1 := {op(`list_elements/big_subsets`(B))} minus {B};

  for C in P1 do
   if not(`is_element/SCP2`(N)(B,C)([Q[B],Q[C]])) then
    reason := [convert(procname,string),"(Q[B],Q[C]) is not in SCP(N)(B,C)",
               eval(Q[B]),eval(Q[C]),N,B,C,reason];
    return false;
   fi;
  od;
 od;

 return true;
end;

######################################################################

`is_equal/FFbar` := (N::posint) -> (A::set) -> proc(Q0,Q1)
 local B,P;
 global reason;

 P := `list_elements/big_subsets`(A);

 for B in P do
  if not(`is_equal/SCP`(N)(B)(Q0[B],Q1[B])) then
   reason := [convert(procname,string),"Q0[B] <> Q1[B]",B,Q0[B],Q1[B],reason];
   return false;
  fi;
 od;

 return true;
end;

######################################################################

`is_leq/FFbar` := (N) -> (A) -> proc(Q0,Q1)
 local B,P;
 global reason;

 P := `list_elements/big_subsets`(A);

 for B in P do
  if not(`is_leq/SCP`(N)(B)(Q0[B],Q1[B])) then
   reason := [convert(procname,string),"Q0[B] is not <= Q1[B]",B,Q0[B],Q1[B],reason];
   return false;
  fi;
 od;

 return true;

end;

######################################################################

`build/FFbar` := (N::posint) -> (A::set) -> proc(RTTu)
 local R,TT,i,j,k,n,m,ij,A0,C,TT1,TTS,TTc,T,Q,Q0,D,u,v,M,L,M0,M1,RT,BS,UU,U,a,b;
 R,TT,u := op(RTTu);
 n := nops(A);
 A0 := {seq(i,i=1..n)};
 C := children_map(A0)(TT);
 TT1 := select(T -> nops(T) > 1,TT);
 TTS := sort(map(T -> sort([op(T)]),[op(TT1)]));
 Q := table():
 for T in TTS do
  D := sort([op(C[{op(T)}])],(U,V) -> (U[1] <= V[1]));
  D := map(U -> sort([op(U)]),D);
  m := nops(D);
  v := [seq(u[max(D[i])],i=1..nops(D)-1)];
  M := [seq(i,i=1..m)];
  Q0 := `build/ICP`(N)({op(M)})([M,v]);
  L := [];
  for k from 1 to N do
   M0 := Q0[k];
   M1 := NULL;
   for ij in M0 do
    i,j := op(ij);
    M1 := M1,seq(seq([R[a],R[b]],a in D[i]),b in D[j]);
   od;
   M1 := {M1};
   L := [op(L),M1];
  od;
  RT := {seq(R[a],a in T)};
  Q[RT] := L;
 od:
 TT1 := map(op,{indices(Q)});
 BS := {op(`list_elements/big_subsets`(A))};
 TTc := BS minus TT1;
 for T in TTc do
  UU := select(U -> T minus U = {},TT1);
  m := min(map(nops,UU));
  UU := select(U -> nops(U) = m,UU);
  U := UU[1];
  Q[T] := `res/ICP`(N)(U,T)(Q[U]);
 od:
 return eval(Q);
end:

`unbuild/FFbar` := (N::posint) -> (A::set) -> proc(Q)
 local n,R,r,i,TT,TT0,u,a,b,UU,d,U,P,k;
 n := nops(A);
 R := `totalise/FFbar/ord`(N)(A)(Q);
 r := table():
 for i from 1 to n do r[R[i]] := i; od;
 TT := `critical_tree/FFbar`(N)(A)(Q);
 TT0 := map(U -> map(a -> r[a],U),TT);
 u := NULL;
 for i from 1 to n-1 do
  a := R[i];
  b := R[i+1];
  UU := select(U -> member(a,U) and member(b,U),TT);
  d := min(map(nops,UU));
  UU := select(U -> nops(U) = d,UU);
  U := UU[1];
  P := Q[U];
  k := 1;
  while k < N and member([b,a],P[k]) do k := k+1; od;
  u := u,k;
 od;
 u := [u];
 return [R,TT0,u];
end:

######################################################################

`is_interior/FFbar` := (N::posint) -> (A::set) -> proc(Q)
 return `is_separated/SCP`(N)(A)(Q[N]);
end;

######################################################################

`inc/ICP/FFbar` := (N::posint) -> (A::set) -> proc(Q0)
 local TT,T,Q;

 Q := table();
 TT := `list_elements/big_subsets`(A);
 for T in TT do
  Q[T] := `res/ACP`(N)(A,T)(Q0);
 od;

 return eval(Q);
end:

######################################################################

`res/FFbar/ICP` := (N::posint) -> (A::set) -> proc(Q)
 if not(`is_interior/FFbar`(N)(A)(Q)) then
  return FAIL;
 fi;

 return Q[A];
end;

######################################################################
# The functions below could be made much more efficient, but for
# the moment we will stick with the most obvious approach.

`is_critical/FFbar` := (N::posint) -> (A::set) -> proc(Q,U)
 local UU,a;

 UU := `top/autorel`(U);

 for a in A minus U do
  if UU minus Q[U union {a}][N] <> {} then
   return false;
  fi;
 od;

 return true;
end;

##################################################

`critical_tree/FFbar` := (N::posint) -> (A::set) -> proc(Q)
 select(U -> `is_critical/FFbar`(N)(A)(Q,U),
        {op(`list_elements/nonempty_subsets`(A))});
end;

##################################################

`rank/FFbar` := (N::posint) -> (A::set) -> proc(Q)
 local TT,TT1,T;

 TT := `critical_tree/FFbar`(N)(A)(Q);
 TT1 := select(T -> nops(T) > 1,TT);
 return add(`rank/ACP`(N)(T)(Q[T])-1,T in TT1);
end;

##################################################

`totalise/FFbar/ord` := (N::posint) -> (A::set) -> proc(Q)
 local C,P,k;
 C := proc(a,b)
  option remember;
  if a = b then return true; fi;
  P := Q[{a,b}];
  for k from 1 to N do
   if not(member([b,a],P[k])) then
    return true;
   fi;
   if not(member([a,b],P[k])) then
    return false;
   fi;
  od;
  return true;
 end:
 return sort([op(A)],C);
end:

##################################################

`random_element/FFbar` := (N::posint) -> (A::set) -> proc()
 local n,R,TT,u,i;

 n := nops(A);
 R := `random_element/ord`(A)();
 TT := `random_element/standard_stasheff_trees`(n)();
 u := [seq(rand(1..N)(),i=1..n-1)];
 return `build/FFbar`(N)(A)([R,TT,u]);
end:

`list_elements/FFbar` := (N::posint) -> proc(A::set)
 local n,RR,R,U,u,i,j,TTT,TT;

 n := nops(A);
 RR := `list_elements/ord`(A);
 U := [[]];
 for i from 1 to n-1 do
  U := [seq(seq([op(u),j],j=1..N),u in U)];
 od;
 TTT := `list_elements/standard_stasheff_trees`(n);

 return([
  seq(seq(seq(
   `build/FFbar`(N)(A)([R,TT,u]),
   u in U),TT in TTT),R in RR)
 ]);
end:

`count_elements/FFbar` := (N::posint) -> (A::set) ->
 nops(A)! * N^(nops(A)-1) *
 `count_elements/standard_stasheff_trees`(nops(A));

`list_ordered_elements/FFbar` := (N::posint) -> proc(A::{set,list})
 local n,A0,R,U,u,i,j,TTT,TT;

 n := nops(A);
 A0 := {op(A)};
 R := [op(A)];
 U := [[]];
 for i from 1 to n-1 do
  U := [seq(seq([op(u),j],j=1..N),u in U)];
 od;
 TTT := `list_elements/standard_stasheff_trees`(n);

 return([
  seq(seq(
   `build/FFbar`(N)(A)([R,TT,u]),
   u in U),TT in TTT)
 ]);
end:

`count_ordered_elements/FFbar` := (N::posint) -> (A::set) ->
 N^(nops(A)-1) *
 `count_elements/standard_stasheff_trees`(nops(A));

######################################################################

`eta/FFbar` := (N::posint) -> (A::set) -> `if`(nops(A) = 1,table(),FAIL);

######################################################################

`gamma/FFbar` := (N::posint) -> (A::set,B::set) -> (p) -> proc(Q,P)
 local R,TT,T,U,M,i;

 R := table();
 TT := `list_elements/big_subsets`(A);

 for T in TT do
  U := map(a -> p[a],T);
  if nops(U) > 1 then
   M := Q[U];
   R[T] := [seq(select(u -> member([p[u[1]],p[u[2]]],M[i]),`top/autorel`(T)),i=1..N)];
  else
   R[T] := eval(P[op(U)][T]);
  fi;
 od;

 return eval(R);
end;

######################################################################

`mu/Fbar/FFbar` := (N::posint) -> (A::set) -> proc(x)
 local Q,TT,T;

 Q := table();

 TT := `list_elements/big_subsets`(A);
 for T in TT do
  Q[T] := `mu/W/ACP`(N)(T)(x[T]);
 od;

 return eval(Q);
end;

######################################################################

`sigma/Fbar/FFbar` := (N::posint) -> (A::set) -> proc(Q)
 local x,TT,T;

 x := table();

 TT := `list_elements/big_subsets`(A);
 for T in TT do
  x[T] := `sigma/ACP/W`(N)(T)(Q[T]);
 od;

 return eval(x);
end;

######################################################################

`describe/FFbar` := (N::posint) -> (A::set) -> proc(Q)
 local TT,TT1,T,s;
 TT := `critical_tree/FFbar`(N)(A)(Q);
 TT1 := select(T -> nops(T) > 1,TT);
 TT1 := sort([op(TT1)],(U,V) -> nops(U) >= nops(V));
 s := "";
 for T in TT1 do
  if s <> "" then s := cat(s,"\n"); fi;
  s := cat(s,`describe/ACP`(N)(T)(Q[T]));
 od:
 return s;
end: