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

`eta/height_functions`:= (A::set) -> `if`(nops(A)=1,table(A=0),FAIL);

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

`gamma/height_functions`:= (A::set,B::set) -> (p) -> proc(j,h)
 local k,P,phi,b0,b1,m,U,pU,b;

 k := table();
 P := `list_elements/nonempty_subsets`(A);

 phi := table();
 for b0 in B do
  m := 1;
  for b1 in B do
   if b1 <> b0 then m := min(m,j[{b0,b1}]); fi;
  od;
  phi[b0] := m;
 od;

 for U in P do
  pU := map(u -> p[u],U);
  if nops(pU) > 1 then
   k[U] := j[pU];
  else
   b := op(pU);
   k[U] := phi[b] * h[b][U];
  fi;
 od;

 return eval(k);
end;

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

`gamma_min/height_functions`:= (A::set,B::set) -> (p) -> proc(j,h)
 local k,P,phi,b0,b1,m,U,pU,b;

 k := table();
 P := `list_elements/nonempty_subsets`(A);

 phi := table();
 for b0 in B do
  m := 1;
  for b1 in B do
   if b1 <> b0 then m := min(m,j[{b0,b1}]); fi;
  od;
  phi[b0] := m;
 od;

 for U in P do
  pU := map(u -> p[u],U);
  if nops(pU) > 1 then
   k[U] := j[pU];
  else
   b := op(pU);
   k[U] := min(phi[b] , h[b][U]);
  fi;
 od;

 return eval(k);
end;


read "../IdentifiabilityODE.mpl";

sys := [
diff(EGF_EGFR(t), t) = reaction_1_k1*EGF_EGFR(t) - reaction_9_k1*EGF_EGFR(t) - reaction_1_k2*EGF_EGFR(t),
diff(EGFR(t), t) = -reaction_1_k1*EGF_EGFR(t) + reaction_1_k2*EGF_EGFR(t) - EGFR(t)*EGFR_turnover + EGFR_turnover*pro_EGFR(t),
diff(pEGFR(t), t) = -reaction_4_k1*pEGFR(t) + reaction_9_k1*EGF_EGFR(t) - pEGFR(t)*Akt(t)*reaction_2_k1 + reaction_3_k1*pEGFR_Akt(t) + pEGFR_Akt(t)*reaction_2_k2,
diff(pAkt_S6(t), t) = pAkt(t)*reaction_5_k1*S6(t) - reaction_6_k1*pAkt_S6(t) - pAkt_S6(t)*reaction_5_k2,
diff(pEGFR_Akt(t), t) = pEGFR(t)*Akt(t)*reaction_2_k1 - reaction_3_k1*pEGFR_Akt(t) - pEGFR_Akt(t)*reaction_2_k2,
diff(pAkt(t), t) = -pAkt(t)*reaction_7_k1 - pAkt(t)*reaction_5_k1*S6(t) + reaction_6_k1*pAkt_S6(t) + reaction_3_k1*pEGFR_Akt(t) + pAkt_S6(t)*reaction_5_k2,
diff(S6(t), t) = pS6(t)*reaction_8_k1 - pAkt(t)*reaction_5_k1*S6(t) + pAkt_S6(t)*reaction_5_k2,
diff(pS6(t), t) = -pS6(t)*reaction_8_k1 + reaction_6_k1*pAkt_S6(t),
diff(Akt(t), t) = pAkt(t)*reaction_7_k1 - pEGFR(t)*Akt(t)*reaction_2_k1 + pEGFR_Akt(t)*reaction_2_k2,
y1(t) = pEGFR(t)*a1 + a1*pEGFR_Akt(t),
y2(t) = a2*pAkt(t) + a2*pAkt_S6(t),
y3(t) = pS6(t)*a3
];
CodeTools[CPUTime](IdentifiabilityODE(sys, GetParameters(sys)));


(*
 Copyright (C) 2020 Susi Lehtola

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)
(* prefix:
  gga_k_lgap_params *params;

  assert(p->params != NULL);
  params = (gga_k_lgap_params * ) (p->params);
*)

(* Equation (20) *)
lgap_f0 := s -> 1 + params_a_kappa*(1-exp(-add(params_a_mu[i]*s^(i), i=1..3))):
lgap_f := x -> lgap_f0(X2S*x):

f := (rs, z, xt, xs0, xs1) ->
  gga_kinetic(lgap_f, rs, z, xs0, xs1):


(*
 Copyright (C) 2020 Susi Lehtola

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_exc *)
(* prefix:
  mgga_c_r2scan_params *params;

  assert(p->params != NULL);
  params = (mgga_c_r2scan_params * )(p->params);
*)

$include "mgga_c_rscan.mpl"
$define lda_c_pw_params
$define lda_c_pw_modified_params
$include "lda_c_pw.mpl"

(* These come from pbe correlation *)
params_a_gamma := (1 - log(2))/Pi^2:
mgamma := params_a_gamma:

(* r2scan values *)
params_a_dp2 := 0.361:

(* Equation (S6) *)
r2scan_alpha := (z, xt, ts0, ts1) -> (t_total(z, ts0, ts1) - xt^2/8) / (K_FACTOR_C*t_total(z, 1, 1) + params_a_eta*xt^2/8):

(* Equation (S26) *)
r2scan_f_alpha_neg := a -> exp(-params_a_c1*a/(1 - a)):
r2scan_f_alpha := (a, ff) -> my_piecewise5(
  a <= 0, r2scan_f_alpha_neg(m_min(a, 0)),
  a <= 2.5, rscan_f_alpha_small(m_min(a, 2.5), ff),
  rscan_f_alpha_large(m_max(a, 2.5))):

(* Equation (S28) *)
r2scan_d := z -> (opz_pow_n(z,5/3) + opz_pow_n(-z,5/3))/2:

(* Equation (S33): beta(rs), this is the same as in gga_c_regtpss *)
beta_a := 0.066724550603149220:
beta_b := 0.1:
beta_c := 0.1778:
mbeta := (rs) -> beta_a*(1 + beta_b*rs)/(1 + beta_c*rs):

(* Equation (S30) *)
w1 := (rs, z) -> exp(-f_pw(rs, z)/(mgamma*mphi(z)^3)) - 1:

(* Equation (S27); note that the paper indexes starting from zero *)
r2scan_dfc2 := ff -> add(i*ff[8-i], i=1..7):

(* According to James Furness, this is LSDA0 - see also Equation (S25) *)
r2scan_elsda0 := (rs, z) -> scan_eclda0(rs)*scan_Gc(z):
(* while LSDA1 is just Perdew-Wang *)
r2scan_elsda1 := (rs, z) -> f_pw(rs, z):

(* Derivatives wrt rs *)
r2scan_delsda0 := (rs, z) -> eval(diff(r2scan_elsda0(x1, x2), x1), [x1=rs, x2=z]):
r2scan_delsda1 := (rs, z) -> eval(diff(r2scan_elsda1(x1, x2), x1), [x1=rs, x2=z]):

(* Equation (S34) *)
r2scan_dy := (rs, z, s) -> r2scan_dfc2(rscan_fc)/(27 * mgamma * r2scan_d(z) * mphi(z)^3 * w1(rs, z)) * (
    + 20*rs*(r2scan_delsda0(rs, z) - r2scan_delsda1(rs, z))
    - 45*params_a_eta*(r2scan_elsda0(rs, z) - r2scan_elsda1(rs, z))
  ) * s^2*exp(-s^4/params_a_dp2^4):

(* Equation (S32) *)
r2scan_y := (rs, z, t) -> mbeta(rs)*t^2/(mgamma*w1(rs, z)):

(* Equation (S31) *)
r2scan_g := (rs, z, s, t) -> 1/(1 + 4*(r2scan_y(rs, z, t) - r2scan_dy(rs, z, s)))^(1/4):

(* Equation (S29) *)
fH := (rs, z, s, t) -> mgamma*mphi(z)^3*log(1 + w1(rs, z)*(1 - r2scan_g(rs, z, s, t))):

(* Now we can build ec1 from (S24) *)
r2scan_ec1 := (rs, z, s, t) -> f_pw(rs, z) + fH(rs, z, s, t):

(* Equation (S35)-(S41) are same as SCAN *)
r2scan_ec0 := (rs, z, s) -> scan_e0(rs, z, s):

(* and the functional itself *)
r2scan_f := (rs, z, xt, xs0, xs1, ts0, ts1) ->
  r2scan_ec1(rs, z, X2S*2^(1/3)*xt, tt(rs, z, xt)) + r2scan_f_alpha(r2scan_alpha(z, xt, ts0, ts1), rscan_fc)*(
    + r2scan_ec0(rs, z, X2S*2^(1/3)*xt) - r2scan_ec1(rs, z, X2S*2^(1/3)*xt, tt(rs, z, xt))):

f := (rs, z, xt, xs0, xs1, us0, us1, ts0, ts1) ->
  r2scan_f(rs, z, xt, xs0, xs1, ts0, ts1):


######################################################################
# trees(A) is the set of trees on A
# (Section sec-trees)

`is_element/trees` := (A::set) -> proc(TT)
 local n,i,j,T,U,is_min,M;
 global reason;

 if not type(TT,set) then
  reason := [convert(procname,string),"TT is not a set",TT];
  return false;
 fi;

 n := nops(TT);
 is_min := table;
 for i from 1 to n do
  is_min[i] := true;
  T := TT[i];
  if not type(T,set) and T <> {} and T minus A = {} then
   reason := [convert(procname,string),"T is not a nonempty subset of A",T,A];
   return false;
  fi;
 od;

 # Now check nesting, and along the way, check which sets are minimal.
 for i from 1 to n do
  T := TT[i];
  for j from 1 to i-1 do
   U := TT[j];
   if T intersect U <> {} then
    if T minus U = {} then
     is_min[j] := false;
    else
     if U minus T = {} then
      is_min[i] := false;
     else
      reason := [convert(procname,string),"T and U are neither disjoint nor nested",T,U];
      return false;
     fi;
    fi;
   fi;
  od;
 od;

 # Check that the union of the minimal sets is A.
 M := NULL;
 for i from 1 to n do
  if is_min[i] then M := M,op(TT[i]); fi;
 od;
 if A minus {M} <> {} then
  reason := [convert(procname,string),"The minimal sets do not cover A",M,A];
  return false;
 fi;

 return true;
end;

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

`is_equal/trees` := (A::set) -> (TT,UU) -> evalb(TT = UU);

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

`is_leq/trees` := (A::set) -> (TT,UU) -> evalb(TT minus UU = {});

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

`is_separated/trees` := (A::set) -> proc(TT)
 local a;
 return evalb({seq({a},a in A)} minus TT = {});
end:

######################################################################
# Suppose we have a set A, a tree TT on A, a set T in TT, and a point a that
# does not lie in A.  We can then construct a new tree on A u {a} by attaching
# a to the node T.  There are two slightly different ways to do this.
# We can either attach a to a new node just above T, or we can attach a to
# the node T itself.  (However, the second version is invalid if |T|=1).

`attach_to_tree` := (A::set) -> proc(TT,T,a,above_)
 local above,CT,SCT,NCT;

 above := `if`(nargs > 3, above_, false);

 if not(`is_element/trees`(A)(TT) and
        member(T,TT) and
        not(member(a,A))) then
  return FAIL;
 fi;

 if nops(T)=1 and not(above) then
  return FAIL;
 fi;

 # Sets that contain (or are equal to) T
 CT := select(U -> (T minus U = {}),TT);

 SCT := CT minus {T};

 # Sets that do not contain T
 NCT := TT minus CT;

 if above then
  return {{a},op(NCT),T,op(map(U -> U union {a},CT))};
 else
  return {{a},op(NCT),op(map(U -> U union {a},CT))};
 fi;
end;

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

`random_element/trees`:= (A::set) -> proc()
 local n,a,A1,T0,T1,T,above;

 # Degenerate cases for small A
 if A={} then return {}; fi;
 if nops(A)=1 then return {A}; fi;

 # If A is not too small, we write it as {a} union A1.  Then we recursively
 # choose a random tree on A1, and attach a to it.

 n := nops(A);
 a := A[rand(1..n)()];
 A1 := A minus {a};
 if n = 2 then
  above := true;
 else
  above := evalb(rand(0..1)() = 1);
 fi;

 T0 := `random_element/trees`(A1)();
 T1 := select(U -> nops(U) > 1,T0);

 if above then
  T := T0[rand(1..nops(T0))()];
 else
  T := T1[rand(1..nops(T1))()];
 fi;

 T := `attach_to_tree`(A1)(T0,T,a,above);

 return T;
end;

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

`big_sets/trees` := (TT) -> select(T -> nops(T) > 1,TT);

######################################################################
# Given a tree TT, this returns a table P indexed by the non-maximal
# elements of TT, such that P[T] is the parent of T in TT.

`parent_map` := (A::set) -> proc(TT)
 local TS,n,i,j,T,U,parent;

 # if not(`is_element/trees`(A)(TT)) then return FAIL; fi;

 TS := sort([op(TT)],(a,b) -> (nops(a) < nops(b)));
 n := nops(TS);
 parent := table();

 for i from 1 to n do
  T := TS[i];
  parent[T] := FAIL;
  for j from i+1 to n do
   U := TS[j];
   if T minus U = {} then
    parent[T] := U;
    break;
   fi;
  od;
 od;

 return eval(parent);
end;

######################################################################
# Given a tree TT, this returns a table C indexed by the
# elements of TT, such that C[T] is the set of children of T in
# TT.  In particular, C[T] = {} if T is minimal.

`children_map` := (A::set) -> proc(TT)
 local parent,children,TT0,T,U;

 parent := eval(`parent_map`(A)(TT));

 children := table();
 for T in TT do
  children[T] := {};
 od:

 TT0 := TT minus {A};

 for T in TT0 do
  U := parent[T];
  if U <> FAIL then
   children[U] := {op(children[U]),T};
  fi;
 od;

 return eval(children);
end;

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

corolla := proc(A::set)
 local a;
 return {A,seq({a},a in A)};
end:


# Kinematik für parallelen Roboter
# Einleitung
# Berechnung der Jacobi-Matrix des parallelen Roboters
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# para -> Berechnung für eine parallelen Roboter
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# kinematics -> Berechnung der Kinematik
# 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
# [Abdellatif2007] Modellierung, Identifikation und robuste Regelung von Robotern mit parallelkinematischen Strukturen
# [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):
with(LinearAlgebra):
#with(ArrayTools):
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_debug := false:
codegen_opt := 2:
codeexport_invdyn := true:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../helper/proc_vec2skew":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", leg_name):
# Definitionen für parallel Roboter laden
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/para_definitions", 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)):
# Kennung des Parametersatzes, für den die Dynamikfunktionen erstellt werden sollen. Muss im Repo und in der mpl-Datei auf 1 gelassen werden, da die folgende Zeile mit einem Skript verarbeitet wird.
codegen_dynpar := 2:
# Link-Index, für den die Jacobi-Matrix aufgestellt wird. Hier wird angenommen, dass der Endeffektor das letzte Segment (=Link) ist. Die Jacobi-Matrix kann hier aber für beliebige Segmente aufgestellt werden. (0=Basis)
LIJAC:=NL-1:
# Ergebnisse der analytischen Jacobi-Matrix (Translatorisch)
read sprintf("../codeexport/%s/tmp/jacobia_transl_%d_maple.m", leg_name, LIJAC):
read "../robot_codegen_definitions/robot_env_par":
b_transl := b_transl:
# Setze Parameter aus serieller Berechnung zu null.
px, py, pz := 0, 0, 0:
alphaxs_base, betays_base, gammazs_base := 0, 0, 0:
rxs_base, rys_base, rzs_base := 0, 0, 0:
# Merke Startzeit für Debug-Ausgabe
st := time():
# Additional Kinematics
# Definition der Koppelpunkte für jedes Bein und der EE-Koordinaten/-Geschwindigkeiten/-Beschleunigungen
tmp := Matrix(3,1,[xP[1],yP[1],zP[1]]):
for j to 3 do:
    if (xE_s(j) = 0) then
      tmp(j,1) := 0:
    end if:
  end do:
for i to N_LEGS do
  P||i := <xP[i];yP[i];zP[i]>;
  for j to 3 do:
    if (xE_s(j) = 0) then
      P||i(j,1) := 0:
    end if:
  end do:
  j := i+1:
  if i <> 1 then
     tmp := <tmp | P||i>:
  end:
end do:
P_i := tmp:
#rhs||i := parse(sprintf("eul%s2r",angleConvLeg))(xE_s(4..6)).P||i+Matrix(xE_s(1..3,1)):
# Jacobi Matrices (JB1/U1) + Derivates
# Berechnung der Jacobi-Matrix der inversen Kinematik: Koppelpunktgeschwindigkeiten P -> Gelenkgeschwindigkeiten
# Abdellatif2007 S.37 unten

transDOF := nops(indets(xE_s(1..3,1))):
JB1 := (parse(sprintf("eul%s2r",angleConvLeg))(frame_A_i(1..3,1)).b_transl)(1..transDOF,1..NQJ_parallel):
try:
  JB1inv := MatrixInverse(JB1):
catch:
  printf("%s. Jacobi-Matrix JB1 ist singulär. Rang ist %d/%d. Abbruch der Berechnungen. Keine Berechnung der PKM-Dynamik möglich.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), Rank(JB1), RowDimension(JB1)):
  quit: # Funktioniert in GUI nicht richtig...
  robot_name := "": # ...Daher auch Löschung des Roboternamens.
end try:
for i from 1 to 3-transDOF do
  JB1inv := <JB1inv|ZeroMatrix(NQJ_parallel,1)>;
end do:
for i from 1 to 3-transDOF do
  JB1 := <JB1;ZeroMatrix(1,NQJ_parallel)>;
end do:
JB1inv := simplify2(JB1inv):
JB1 := simplify2(JB1):

# Berechnung der Matrix Ui: EE-Geschwindigkeiten -> Koppelpunktgeschwindigkeiten P. i steht für den Index des jeweiligen Beines
# Abdellatif2007 S.21 (2.21)
for i to N_LEGS do
  r||i := P||i:
  if angleConv = X_Y_Z then
     r||i := vec2skew(rotx(xE_t(4)).roty(xE_t(5)).rotz(xE_t(6)).r||i):
  elif angleConv = Z_Y_X then
     r||i := vec2skew(rotz(xE_t(4)).roty(xE_t(5)).rotx(xE_t(6)).r||i):
  end:

  r||i := vec2skew(parse(sprintf("eul%s2r",angleConvLeg))(xE_t(4..6)).P||i);
  ones := Matrix(3,3,shape=identity):
  U||i := simplify(<ones|-r||i>):
  U||i||D := simplify(diff~(U||i,t)):      #dU berechnen
  #U||i||D := convert_t_s(U||i||D): #dU berechnen
  #U||i := convert_t_s(U||i):
end do:

# Ermittlung des Robotertyps, um später die Gesamt-Jacobi-Matrix reduzieren zu können
robotType := 1:
counter := 0:
for i from 4 to 6 do
  if xE_t(i) = 0 then
    counter := counter + 1:
  end if:
end do:
if counter = 2 then
  robotType := 2:
  for i from 1 to 3 do
    if not(xE_t(i+3) = 0) then
      rotPlanar := substring(angleConvLeg,i);
    end if:
  end do:
elif counter = 3 then
  robotType := 3:
end if:
rotPlanar:

# Erstelle die Matrizen für jedes Bein.
U_i := Copy(U1):
UD_i := Copy(U1D):
ROW := RowDimension(U1):
COLUMN := ColumnDimension(U1):
# Substituiere die zeitabhängigen EE-Koordinaten mit den oben definierten zeitunabhängigen Koordinaten
for i to N_LEGS do
  for j to 3 do
    for k to 6 do
      for l from 4 to 6 do
        U||i(j,k) := subs(xE_t(l)=xE_s(l),U||i(j,k)):
        U||i||D(j,k) := subs({xED_t(l)=xED_s(l),xE_t(l)=xE_s(l)},U||i||D(j,k)):
      end do:
    end do:
  end do:
  U_i(1..ROW,1..COLUMN,i) := U||i:
  UD_i(1..ROW,1..COLUMN,i) := U||i||D:
end do:
printf("[%s] Koppelpunkt-Jacobi-Matrizen (bezogen auf B) der Beinketten berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Berechnung von JB1D
JB1 := convert_s_t(JB1):
JB1D := diff~(JB1,t):
JB1D := simplify(JB1D):
JB1D := simplify(convert_t_s(JB1D)):
JB1 := simplify(convert_t_s(JB1)):
printf("[%s] Zeitableitung der Koppelpunkt-Jacobi-Matrizen (bezogen auf B) der Beinketten berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
JB1inv := convert_s_t(JB1inv):
JB1Dinv := diff~(JB1inv,t):
JB1Dinv := convert_t_s(JB1Dinv):
JB1inv := convert_t_s(JB1inv):
printf("[%s] Zeitableitung der inversen Koppelpunkt-Jacobi-Matrizen (bezogen auf B) der Beinketten berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):

# Berechne Jacobi-Matrizen für jedes Bein
# Dupliziere alle berechneten Matrizen. i steht für den Index des jeweiligen Beines

JBinv_i := Copy(JB1inv):
JBD_i := Copy(JB1D):
JBDinv_i := Copy(JB1Dinv):
JB_i := Copy(JB1):
#Jinv_DoThanh_i := Copy(Jinv_DoThanh||1):
ROW := RowDimension(JB1):
COLUMN := ColumnDimension(JB1):
for i to N_LEGS do
  JB||i||D := Copy(JB1D):
  JB||i||Dinv := Copy(JB1Dinv):
  JB||i||inv := Copy(JB1inv):
  JB||i := Copy(JB1):
end do:

# 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 ROW do
		for j to COLUMN do
			for l to 3 do
				JB||k||D(i,j):=subs({frame_A_i(l,1)=frame_A_i(l,k)},JB||k||D(i,j)):
				JB||k||inv(j,i):=subs({frame_A_i(l,1)=frame_A_i(l,k)},JB||k||inv(j,i)):
				JB||k||Dinv(j,i):=subs({frame_A_i(l,1)=frame_A_i(l,k)},JB||k||Dinv(j,i)):
				JB||k(i,j):=subs({frame_A_i(l,1)=frame_A_i(l,k)},JB||k(i,j)):
			end do:
      		for m to NQJ_parallel do #alpha
				#tmp := VARS(m):
        			n := m + (k-1)*NQJ_parallel:
        			JB||k||D(i,j):=subs({qJD_i_s(m,1)=qJD_i_s(m,k),qJ_i_s(m,1)=qJ_i_s(m,k)},JB||k||D(i,j)):
       			JB||k||inv(j,i):=subs({qJ_i_s(m,1)=qJ_i_s(m,k)},JB||k||inv(j,i)):
       			JB||k||Dinv(j,i):=subs({qJD_i_s(m,1)=qJD_i_s(m,k),qJ_i_s(m,1)=qJ_i_s(m,k)},JB||k||Dinv(j,i)):
        			JB||k(i,j):=subs({qJ_i_s(m,1)=qJ_i_s(m,k)},JB||k(i,j)):
     		end do:
    		end do:
  	end do:
  	JB_i(1..ROW,1..COLUMN,k) := JB||k:
  	JBD_i(1..ROW,1..COLUMN,k) := JB||k||D:
  	JBinv_i(1..COLUMN,1..ROW,k) := JB||k||inv:
  	JBDinv_i(1..COLUMN,1..ROW,k) := JB||k||Dinv:
  	for i to N_LEGS do
		for j to 6 do
			#Jinv_DoThanh||k(i,j):=subs({frame_A_i(l,1)=frame_A_i(l,k)},Jinv_DoThanh||k(i,j)):
		end do:
		for m to NQJ_parallel do
			n := m + (k-1)*NQJ_parallel:
			#Jinv_DoThanh||k(i,j):=subs({qJ_i_s(m,1)=qJ_i_s(m,k)},Jinv_DoThanh||k(i,j)):
		end do:
	end do:
	#Jinv_DoThanh_i(1..COLUMN,1..ROW,k) := Jinv_DoThanh||k:
end do:
printf("[%s] Jacobi-Matrizen der Beinketten berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Gesamt Jacobi-Matrix
# Berechnung der inv. Jacobi-Matrix: EE-Geschwindigkeiten -> aktive Gelenkgeschwindigkeiten
# Abdellatif2007 S.21 (2.22)
if type(AKTIV,scalar) then
  AKTIV := Matrix(N_LEGS,1,AKTIV);
end if:
Tmp := simplify(JB1inv[AKTIV(1,1),1..3].U1):
for i from 2 to N_LEGS do
  Tmp := <Tmp;JB||i||inv[AKTIV(i,1),1..3].U||i>:
end do:
Jinv := Tmp:

# Reduziere die Gesamt-Jacobi-Matrix auf Freiheitsgrade des Roboters
printf("[%s] Starte Reduzierung der Gesamt-Jacobi-Matrix. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
IdentMat := IdentityMatrix(6,6):
IdentMatMas := IdentityMatrix(6,6):
counter := 0:
for i to 3 do
  if not(xE_t(i) = 0) then
    if counter = 0 then
      counter := 1;
      JinvRed := Column(Jinv, i);
      pivotMat := IdentMat(i,..);
    else
      JinvRed := <JinvRed|Column(Jinv, i)>;
      pivotMat := <pivotMat;IdentMat(i,..)>;
    end if;
  end if:
end do:
if robotType = 2 then
  if rotPlanar = z then
    JinvRed := <JinvRed|Column(Jinv, 6)>;
    pivotMat := <pivotMat;IdentMat(6,..)>;
  elif rotPlanar = y then
    JinvRed := <JinvRed|Column(Jinv, 5)>;
    pivotMat := <pivotMat;IdentMat(5,..)>;
  elif rotPlanar = x then
    JinvRed := <JinvRed|Column(Jinv, 4)>;
    pivotMat := <pivotMat;IdentMat(4,..)>;
  end if;
elif robotType = 1 then
  if counter = 0 then
    JinvRed := Jinv(..,1..3);
    pivotMat := IdentMat(4..6,..);
  else
    JinvRed := <JinvRed|Jinv(..,4..6)>;
    pivotMat := <pivotMat;IdentMat(4..6,..)>;
  end if;
end if;
counter := 0:
for i from 1 to 6 do
  if not(xE_t(i) = 0) then
    if counter = 0 then
      counter := 1;
      pivotMatMas := IdentMat(i,..);
    else
      pivotMatMas := <pivotMatMas;IdentMat(i,..)>;
    end if;
  end if:
end do:
Jinv := JinvRed:
Jinv := simplify(Jinv):
# Export
printf("[%s] Beginne mit Code-Export. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Maple-Export
save pivotMat, pivotMatMas, P_i, Jinv, JB_i, JBD_i, JBinv_i, JBDinv_i, U_i, UD_i, sprintf("../codeexport/%s/tmp/kinematics_%s_platform_maple.m", robot_name, base_method_name):
MatlabExport(Jinv, sprintf("../codeexport/%s/tmp/Jinv_para_matlab.m", robot_name), codegen_opt):


`is_element/tree_Fbar_alt` := (N::posint) -> (A::set) -> (TT) -> proc(x)
 local TT1,P,children,T,UU,UU1;
 global reason;

 TT1 := select(T -> nops(T) > 1,TT);

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

 P := sort(map(sort,map(op,{indices(x)})));

 if P <> TT1 then
  reason := [convert(procname,string),"x is not indexed by the big sets in TT",P,TT1];
  return false;
 fi;

 children := children_map(A)(TT);

 for T in TT1 do
  UU := children[T];
  UU1 := select(U -> (nops(U) > 1),UU);

  if not(`is_element/D/Fbar`(N)(UU1,UU)(x[T])) then
   reason := [convert(procname,string),"x[T] is not in D(C1,C)",x[T],N,T,reason];
   return false;
  fi;
 od;

 return true;
end;

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

`is_equal/tree_Fbar_alt` := (N::posint) -> (A::set) -> (TT) -> proc(x,y)
 local TT1,P,children,T,UU,UU1;

 TT1 := select(T -> nops(T) > 1,TT);

 children := children_map(A)(TT);

 for T in TT1 do
  UU := children[T];
  UU1 := select(U -> (nops(U) > 1),UU);

  if not(`is_equal/D/Fbar`(N)(UU1,UU)(x[T],y[T])) then
   return false;
  fi;
 od;

 return true;
end;

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

`random_element/tree_Fbar_alt` := (N::posint) -> (A::set) -> (TT) -> proc()
 local x,TT1,children,T,UU,UU1;

 x := table();

 TT1 := select(T -> nops(T) > 1,TT);
 children := children_map(A)(TT);

 for T in TT1 do
  UU := children[T];
  UU1 := select(U -> (nops(U) > 1),UU);
  x[T] := `random_element/D/Fbar`(N)(UU1,UU)();
 od;

 return eval(x);
end;

######################################################################
# From tree_Fbar_alt to tree_Fbar

`theta/tree_Fbar` := (N::posint) -> (A::set) -> (TT) -> proc(ty)
 local TT1,children,z,t,y,C,n,pi,w,w0,a,m,T,U;

 TT1 := select(T -> nops(T) > 1,TT);
 TT1 := sort([op(TT1)],(U1,U2) -> (nops(U1) < nops(U2)));
 children := children_map(A)(TT);
 z := table();
 t := table();
 y := table();

 for T in TT1 do
  z[T] := table();
  t[T],y[T] := op(ty[T]);
  C := children[T];
  n := combine(simplify(sqrt(add(`norm_2/R`(N)(y[T][a])^2,a in C))));
  pi := table();
  for U in C do
   for a in U do
    pi[a] := U;
   od;
  od;
  w := table();
  w0 := [0$N];
  for a in T do
   w[a] := y[T][pi[a]];
   w0 := combine(simplify(w0 +~ w[a]));
  od;
  w0 := combine(simplify(w0 /~ nops(T)));
  for a in T do
   w[a] := combine(simplify(w[a] -~ w0));
  od;
  m := combine(simplify(sqrt(add(`norm_2/R`(N)(w[a])^2,a in T))));
  if is(m > 0) then
   for a in T do
    w[a] := combine(simplify((n/m) *~ w[a]));
   od;
  fi;
  for U in C do
   if nops(U) > 1 then
    for a in U do
     w[a] := combine(simplify(w[a] +~ t[T][U] *~ z[U][a]));
    od;
   fi;
  od;
  z[T] := eval(w);
 od:

 return eval(z);
end;

######################################################################
# From tree_Fbar to tree_Fbar_alt

`phi/tree_Fbar` := (N::posint) -> (A::set) -> (TT) -> proc(x)
 local TT1,children,x1,t,y0,y,ty,C,C1,n,m,T,U,a;

 TT1 := select(T -> nops(T) > 1,TT);
 children := children_map(A)(TT);
 x1 := table();
 t  := table();
 y0 := table();
 y  := table();
 ty := table();

 for T in TT1 do
  x1[T] := `normalise_2/SW`(N)(T)(x[T]);
  t[T]  := table();
  y0[T] := table();
  y[T]  := table();
  C := children[T];
  C1 := select(U -> nops(U) > 1,C);
  n := 0;
  for U in C do
   y0[T][U] := simplify(expand(`sum/vector_functions`(N)(U)(x1[T]) /~ nops(U)));
   n := simplify(expand(n + `norm_2/R`(N)(y0[T][U])^2));
   if nops(U) > 1 then
    t[T][U]  := simplify(expand(sqrt(add(`norm_2/R`(N)(x1[T][a] -~ y0[T][U])^2,a in U))));
   fi;
  od;
  if is(n > 0) then
   y[T] := `normalise_2/SW`(N)(C)(y0[T]);
   m := combine(simplify(expand(rationalize(sqrt(1 - add(t[T][U]^2,U in C1))))));
   for U in C do
    y[T][U] := combine(simplify(expand(m *~ y[T][U])));
   od;
  else
   for U in C do
    y[T][U] := [0$N];
   od;
  fi;
  ty[T] := [eval(t[T]),eval(y[T])];
 od:

 return eval(ty);
end:


# gpgcd-bezout-test 2nd variation
# GPGCD test routines for Bezout resultant
# Akira Terui, Boming Chi, October 2018

with (LinearAlgebra);
with (PolynomialTools);
with (ArrayTools);
with (VectorCalculus);

# The update function
bezout_update := proc (Btemplate, J, degH, F0, G0, V0, F1, G1, V1, alpha)
local i, j, dimF, dimG, dimV, dimVV1, F1poly, G1poly, B1, VV1, Q1, P1, R1, dim, BB, vlist, varlist, F0grad, F1grad, subslistB, subslistV, sublist, J1, Jrow, Jcol, A1, D1, DD1, DD2, VV2, F2, G2, V2, Normfg, st;
    # Jrow, Jcol, A1, R1, D1, f, k, B2, V2;

    # inputs:

    # B:            Bezout matrix (template)
    # J:             Jacobi matrix (template)
    # degH:     degree of the approximate GCD
    # F0, G0:    initial coefficient vector of F and G
    # F1, G1:    current coefficient vector of F and G
    # V0:          initial singular vector of the Bezout matrix
    # V1:          current singular vector of the Bezout matrix
    # alpha:      step width for an iteration

    # outputs:

    # F2, G2:    new (updated) coefficient vector of F and G
    # V2:          new (updated) singular vector of the Bezout matrix
    # Normfg:  the distance between F1, G1 and F2, G2

    # find dimension of initial coefficient vector( F0, G0 and V0)

    dimF := Dimension(F0);
    dimG := Dimension(G0);
    dimV := Dimension(V0);
    userinfo(2, bezout_update, `[dimF, dimG, dimV] =`,
             print([dimF, dimG, dimV]));

    # construct polynomials from coefficient vector( F1 and G1)

    st := time();
    F1poly := FromCoefficientVector(F1, x);
    G1poly := FromCoefficientVector(G1, x);
    st := time() - st;
    userinfo(1, bezout_update, `stpoly=`, print(st));
    userinfo(2, bezout_update, `F1poly=`, print(F1poly));
    userinfo(2, bezout_update, `G1poly=`, print(G1poly));

    # construct substitution table for B1 & J1

    st := time();
    sublist := [seq(f[j] = F1[j+1], j=0..(dimF-1)), seq(g[j] = G1[j+1], j=0..(dimG-1)), seq(v[j] = V1[j], j=1..dimV)];
    st := time() - st;
    userinfo(1, bezout_update, `stsublist=`, print(st));
    userinfo(2, bezout_update, `sublist = `, print(sublist, whattype(sublist)));

    # construct Bezout Matrix of F1 and G1 -> B1

    st := time();
    B1 := BezMatrix(F1poly, G1poly, x);
    userinfo(2, bezout_update, `B1 =`, print(B1));
    st := time() - st;
    userinfo(1, bezout_update, `stB1=`, print(st));

    # construct vector of objective function

    st := time();
    VV1 := <F1, G1, V1>;
    userinfo(2, bezout_update, `VV1 =`, print(VV1, whattype(VV1)));
    dimVV1 := dimF + dimG + dimV;
    st := time() - st;
    userinfo(1, bezout_update, `stVV1=`, print(st));

    # evaluate the constraint value -> Q1

    st := time();
    Q1 := B1 . V1[ 1 .. dimF-1 ];
    st := time() - st;
    userinfo(1, bezout_update, `stQ1=`, print(st));
    userinfo(2, bezout_update, `Q1 =`, print(Q1, whattype(Q1)));

    st := time();
    P1 := < (F1 - F0), (G1 - G0), Vector(dimV, datatype=double) >;
    st := time() - st;
    userinfo(1, bezout_update, `stP1=`, print(st));
    userinfo(2, bezout_update, `P1 =`, print(P1, whattype(P1)));

    # construct right-hand-side vector -> R1

    st := time();
    R1 := - <P1, Q1>;
    st := time() - st;
    userinfo(1, bezout_update, `stR1=`, print(st));
    userinfo(2, bezout_update, `R1 =`, print(R1, whattype(R1)));

    # construct Jacobian matrix -> J1

    st := time();
    J1 := subs(sublist, J);
    st := time() - st;
    userinfo(1, bezout_update, `stJ1=`, print(st));
    userinfo(2, bezout_update, `J1 = `, print(J1, whattype(J1)));

    # construct coefficient matrix for the linear system -> A1

    st := time();
    Jrow, Jcol := Dimension(J);
    A1 := Matrix(Jrow+Jcol);
    for i from 1 to Jcol do A1(i,i):=1 end do:
    for i from 1 to Jrow do for j from 1 to Jcol do A1(i+Jcol,j):=J1(i,j) end do end do:
    for i from 1 to Jcol do for j from 1 to Jrow do A1(i,j+Jcol):=-J1(j,i) end do end do:
    st := time() - st;
    userinfo(1, bezout_update, `stA1=`, print(st));
    userinfo(2, bezout_update, `A1 =`, print(A1));

    # solve the linear system: A1 . D1 == R1

    st := time();
    D1 := LinearSolve(A1, R1);
    st := time() - st;
    userinfo(1, bezout_update, `stD1=`, print(st));
    userinfo(2, bezout_update, `D1 =`, print(D1));

    DD1 := D1[1..dimVV1];
    userinfo(2, bezout_update, `DD1 =`, print(DD1));

    DD2 := D1[1..(dimF+dimG)];
    userinfo(2, bezout_update, `DD2 =`, print(DD2));

    st := time();
    VV2 := (VV1 + alpha * DD1)[..,1];
    st := time() - st;
    userinfo(1, bezout_update, `stVV2=`, print(st));
    userinfo(2, bezout_update, `VV2 =`, print(VV2, whattype(VV2)));

    F2 := VV2[1..dimF];
    G2 := VV2[(dimF + 1)..(dimF + dimG)];
    V2 := VV2[(dimF + dimG + 1)..(dimVV1)];
    Normfg := LinearAlgebra:-Norm(DD2,2);
    userinfo(2, bezout_update, `Normfg =`, print(Normfg));

    return F2, G2, V2, Normfg;

end proc:

# The initial function
bezout_gpgcd := proc(F, G, x, degH, stopcriterion, alpha)
local degF, degG, hh, Ftemplate, Gtemplate, Btemplate, V, BV, i, j, m, n, J, Fcoef, Gcoef, B, V0, VV0, v0, st, st0, st1, st2, ff, gg, DF, DG, DD, numofiteration, sublist, B0;

    # inputs:

    # F: the initial polynomial F
    # G: the initial polynomial G
    # degH: the degree of the approximate GCD
    # alpha: step width for an iteration

    # outputs:

    # ff, gg: the polynomials finally we get
    # hh: the approximate GCD finally we get
    # DF, DG: the perturbation of ff and gg
    # DD: the whole perturbation
    # numofiteration: number of iterations
    # B0: the Bezout Matrix of ff and gg

    # Construct the Bezout matrix of the initial polynomials

    st0 := time();
    degF := degree(F,x);
    degG := degree(G,x);
    # if degF < degG then degF, degG := degG, degF; F, G := G, F end if;
    userinfo(2, bezout_update, `degG =`, print(degG));
    Ftemplate := add(f[i]*x^i, i=0..degF);
    Gtemplate := add(g[i]*x^i, i=0..degG);
    userinfo(2, bezout_update, `Gtemplate =`, print(Gtemplate));    st := time();
    Btemplate := BezMatrix(Ftemplate, Gtemplate, x);

    # Construct the Jacobian matrix

    st := time() - st;
    userinfo(1, bezout_update, `stBtemplate=`, print(st));
    V := Vector(degF,symbol=v);
    BV := Vector(degF);
    for i from 1 to degF do BV[i] := add(Btemplate[i,j]*V[j], j=1..degF) end do;
    userinfo(2, bezout_update, `BV =`, print(BV));
    st := time();
    J := bezout_jacobian(degF, degG, Btemplate);
    st := time() - st;
    userinfo(1, bezout_update, `stJtemplate=`, print(st));
    userinfo(1, bezout_update, `J =`, print(J));

    # Construct coefficient vectors

    Fcoef := PolynomialTools:-CoefficientVector(F, x);
    Gcoef := PolynomialTools:-CoefficientVector(G, x);

    # Construct singular vector of the d-th from the right side

    B := BezMatrix(F, G, x);
    V0 := LinearAlgebra:-Transpose(LinearAlgebra:-SingularValues(B, output='Vt'));
    v0 := V0[..,-degH];
    userinfo(2, bezout_update, `v0 =`, print(v0));
    st1 := time() - st0;
    userinfo(1, bezout_update, `stbeforeiteration=`, print(st1));
    ff, gg, hh, DF, DG, DD, numofiteration, B0 := bezout_iteration(Btemplate, J, degF, degG, degH, Fcoef, Gcoef, v0, Fcoef, Gcoef, v0, stopcriterion, 1.0, 0);
    st2 := time() - st0;
    userinfo(1, bezout_update, `stwholeiteration=`, print(st2));

    return ff, gg, hh, DF, DG, DD, numofiteration, B0;
    # return B, J, V0, v0
end proc:

# The Jacobian Matrix
bezout_jacobian := proc (m, n, Btemplate)
local J, i, j;

    # inputs:
    # degF, degG： the degree of polynomials F and G

    J := Matrix(m, 2*m + n + 2);

    for i from 1 to m do for j from i+1 to m+1 do J(i,j):= -g[i-1]*v[j-1] end do end do;
    for i from 2 to m do for j from i+1 to m+1 do J(i,j):= J(i,j)+J(i-1,j-1) end do end do;

    for j from 1 to n do for i from j to n do J(i,j):= g[i]*v[j] end do end do;
    for j from n-1 by -1 to 1 do for i from n-1 by -1 to j do J(i,j):= J(i,j)+J(i+1,j+1) end do end do;

    for i from 1 to n+1 do for j from i+m+2 to m+n+2 do J(i,j):= f[i-1]*v[j-m-2] end do end do;
    for i from 2 to n+1 do for j from i+m+2 to m+n+2 do J(i,j):= J(i,j)+J(i-1,j-1) end do end do;

    for j from m+2 to 2*m+1 do for i from j-m-1 to m do J(i,j):= -f[i]*v[j-m-1] end do end do;
    for j from 2*m by -1 to m+2 do for i from m-1 by -1 to j-m-1 do J(i,j):= J(i,j)+J(i+1,j+1) end do end do;

    for j from m+n+3 to 2*m+n+2 do for i from 1 to m do J(i,j):= Btemplate(i,j-m-n-2) end do end do;
    return J
end proc:

# The iteration function
bezout_iteration := proc (Btemplate, J, degF, degG, degH, F0, G0, V0, F1, G1, V1, stopcriterion, alpha, numofiteration)
local FF, GG, VV, Normfg, B, ff, gg, f0, g0, DF, DG, DD, hh, st, numofiteration0, sublist;

    # inputs:

    # Btemplate:       the template of the Bezout Matrix
    # J:                      the template of the Jacobian Matrix
    # degF, degG:     the degree of the initial polynomials
    # degH:               the degree of the approximate GCD
    # F0, G0:              initial coefficient vector of F and G
    # F1, G1:              current coefficient vector of F and G
    # V0:                    initial singular vector of the Bezout matrix
    # V1:                    current singular vector of the Bezout matrix
    # alpha:                step width for an iteration
    # stopcriterion:     the stop criterion
    # numofiteration: number of current iteration( initial number of iteration: 0)

    # outputs:

    # ff, gg: the polynomials finally we get
    # hh: the approximate GCD finally we get
    # DF, DG: the perturbation of ff and gg
    # DD: the whole perturbation
    # numofiteration: number of current iteration
    # B0: the Bezout Matrix of ff and gg

    FF, GG, VV, Normfg := bezout_update(Btemplate, J, degH, F0, G0, V0, F1, G1, V1, alpha);
    if Normfg < stopcriterion then
    userinfo(2, bezout_update, `FF=`, print(FF));
    st := time();
    f0 := FromCoefficientVector(F0,x);
    g0 := FromCoefficientVector(G0,x);
    ff := FromCoefficientVector(FF,x);
    gg := FromCoefficientVector(GG,x);
    DF := Norm(PolynomialTools:-CoefficientVector(ff-f0,x),2);
    DG := Norm(PolynomialTools:-CoefficientVector(gg-g0,x),2);
    DD := sqrt(DF^2+DG^2);
    sublist := [seq(f[j] = FF[j+1], j=0..degF), seq(g[j] = GG[j+1], j=0..degG)];
    B := BezMatrix(ff, gg, x);
    userinfo(2, bezout_update, `B=`, print(B));
    hh := bezout_gcd(B, max(degF, degG), degH);
    st := time() - st;
    userinfo(1, bezout_update, `stafteriteration=`, print(st));
        return ff, gg, hh, DF, DG, DD, numofiteration, B;
    else
    numofiteration0 := numofiteration + 1;
    userinfo(2, bezout_update, `numofiteration0 =`, print(numofiteration0));
    return bezout_iteration(Btemplate, J, degF, degG, degH, F0, G0, V0, FF, GG, VV, stopcriterion, alpha, numofiteration0); end if
end proc:

    # Find Bezout Matrix from coefficient vector

BezMatrix := proc(F, G, x)
local degF, degG, Bez, Bez1, i, j, fv, gv, gv0;

    # inputs:
    # F:    the polynomial F
    # G:    the polynomial G

    # outputs:

    # Bez:    the Bezout Matrix of F and G

    degF := degree(F, x);
    degG := degree(G, x);
    fv := CoefficientVector(F, x);
    gv0 := CoefficientVector(G, x);
    gv := Vector(degF+1);
    for i from 1 to degG+1 do gv(i):=gv0(i) end do;

    # let the degree of F be the larger one

    #if degF < degG then F, G := G, F ; degF, degG := degG, degF end if;

    # construct the Bezout Matrix

    Bez := Matrix(degF);

    for j from 1 to degF do
       for i from 1 to j do
          Bez(i,j) := fv[i]*gv[j+1]-gv[i]*fv[j+1]
       end do
    end do;

    for i from 2 to degF-1 do
       for j from i to degF-1 do
          Bez(i,j) := Bez(i,j)+Bez(i-1,j+1)
        end do
    end do;

    for j from 1 to degF-1 do
       for i from j+1 to degF do
          Bez(i,j) := Bez(j,i)
       end do
    end do;

    return Bez
end proc:

# Find norm between two polynomials

normpoly := proc(f, g, x)
local n;

    n := Norm(CoefficientVector(f-g, x),2);
    return n
end proc:

# Find GCD of polynomials from their Bezout Matrix

bezout_gcd := proc (B, n, h)
local p, l, u, b, i, j, X, k, y, CoeffVector, gcdB;

    # inputs:
    # B: the Bezout Matrix
    # n: the degree of the Bezout Matrix
    # k: the degree of GCD

    # outputs:

    # gcdB: the GCD calculated by the Bezout Matrix

    p,l,u:=LUDecomposition(B[..,h+1..n]);
    CoeffVector := Vector(h+1);
    CoeffVector(h+1) := 1;
    for k from 1 to h do
        b:=Transpose(p).B[..,k];
        y:=Vector(1..n);
        for i from 1 to n do y(i):=b(i)-add(l[i,j]*y[j],j=1..i-1) end do:
        X:=Vector(1..n-h);
        for i from 1 to n-h do X(n-h+1-i):=(y(n-h+1-i)-add(u[n-h+1-i,j]*X[j],j=n-h+2-i..n-h))/u(n-h+1-i,n-h+1-i) end do:
        CoeffVector(k) := X(1);
    end do:
    #p,l,u:=LUDecomposition(B[1..n-h,h+1..n]);
    #CoeffVector := Vector(h+1);
    #CoeffVector(h+1) := 1;
    #for k from 1 to h do
    #    LinearSolve([p,l,u],B[..,k],method=LU);
    #end do:
    gcdB := FromCoefficientVector(CoeffVector,x);
    return gcdB
end proc:


with(RegularChains):
with(SemiAlgebraicSetTools):
with(ListTools):

EvalCertification := module()
option package;

export HolderInformationForPolynomial,
	   HolderInformationForExponential,
	   HolderInformationForRationalPolynomial,
	   EstimateRootsAndCertifyEvaluations,
	   ExpandToImaginaryAndReal;

#ExpandToImaginaryAndReal,
local SqrFreeFactorization,
	  SingleEqualityToEstimate,
	  BoxEqualitiesToEstimate,
	  EstimateAndClassifySolutions,
	  HolderDataToCertificationTolerance,
	  EvaluateFunctionAtRoot;

# Compute a square free factorization of a polynomial
# along with multiplicities
SqrFreeFactorization := proc(F)
	local FsqrFree, fa, CurrOutput, j, k; FsqrFree := sqrfree(F);
	FsqrFree := FsqrFree[2]; CurrOutput := [];
	for j to nops(FsqrFree) do
		fa := ifelse(type(factor(FsqrFree[j][1]), `+`), [FsqrFree[j][1]], convert(factor(FsqrFree[j][1]), list));
		for k to nops(fa) do
			CurrOutput := [op(CurrOutput), [fa[k], FsqrFree[j][2]]];
		end do;
	end do;
	return CurrOutput;
end proc;

# Expand a list of polynomials to list with real and
# imaginary parts separated
ExpandToImaginaryAndReal := proc(F)
	local vars, imaginary_variables, expanded_equations, i;
	vars := indets(F);
	imaginary_variables := [seq(b[i], i = 1 .. nops(vars))];
	expanded_equations := expand(subs([seq(vars[i] = Complex(vars[i], imaginary_variables[i]), i = 1 .. nops(vars))], F));
	return [[op(subs(I = 0, expanded_equations)), op([seq(coeff(expanded_equations[i], I), i = 1 .. nops(expanded_equations))])], [seq(vars[i]^2 + imaginary_variables[i]^2 - 1, i = 1 .. nops(vars))]];
end proc;

SingleEqualityToEstimate := proc(equality)
	return ifelse(type(rhs(equality), 'list'), 1/2*rhs(equality)[2] + 1/2*rhs(equality)[1], rhs(equality));
end proc;

BoxEqualitiesToEstimate := proc(equalities)
	local reduced_equalities;
	reduced_equalities := map(SingleEqualityToEstimate, equalities);
	return Complex(reduced_equalities[1], reduced_equalities[2]);
end proc;

# Estimate the solution of univarite polynomial F and classify
# by whether the imaginary norm is > 1, < 1, or = 1
EstimateAndClassifySolutions := proc(F, epsilon)
	# INPUT: Squarefree polynomial F in format [polynomial,power],
	# estimation error epsilon

	# OUTPUT: [list_of_estimates_for_norm_1_solutions,list_of_estimates_for_norm_<_1_solutions,
	# 		   list_of_estimates_for_norm_>_1_solutions]
	local actual_polynomial, system_with_imaginary_parts, equations,
		  norms, R, norm_1_boxes, norm_1_values, norm_greater_boxes,
		  norm_greater_values, norm_less_boxes, norm_less_values, i,
		  norm_1_values_refined, norm_greater_values_refined, norm_less_values_refined;
	actual_polynomial := [F[1]];
	system_with_imaginary_parts := ExpandToImaginaryAndReal(actual_polynomial);
	equations := system_with_imaginary_parts[1];
	norms := system_with_imaginary_parts[2];
	R := PolynomialRing([seq(indets(norms)[i], i = 1 .. nops(indets(norms)))]);
	norm_1_boxes := RealRootIsolate([op(equations)], [], [], [], R, 'abserr' = epsilon);
	norm_1_values := map(BoxValues, norm_1_boxes, R);
	norm_1_values_refined := map(BoxEqualitiesToEstimate, norm_1_values);
	norm_1_values_refined := [seq(Record("value"=norm_1_values_refined[i]), i = 1 .. nops(norm_1_values_refined))];
	return [op(norm_1_values_refined)];
end proc;

# Compute local Lipschitz constant for univarite polynomial F at a point
# given an estimate for that point and a bound on that estimate's accuracy
HolderInformationForPolynomial := proc(F, PointEstimate, EstimateAccuracy)
	# INPUT: Univariate polynomial F,
	# an estimate PointEstimate of a point in F's domain (could be real or complex),
	# an upper bound EstimateAccuracy on the distance from PointEstimate to the
	# actual root

	# OUTPUT: A Record of the form [exponent,constant] such that
	# F is certifiably Holder with those values
	local deg, Derivatives, HolderConstant, HolderExponent, i;
	deg := degree(F);
	if deg > 0 then
		Derivatives := Vector();
		Derivatives(1) := diff(F, indets(F)[1] $ 1);
		for i from 2 to deg do
	    	Derivatives(i) := diff(Derivatives(i - 1), indets(F)[1] $ 1);
		end do;
		Derivatives := convert(Derivatives, list);
		HolderConstant := add((EstimateAccuracy)^(i - 1)*abs(subs(indets(F)[1] = PointEstimate, Derivatives[i]))/(i - 1)!, i = 1 .. deg);
	else HolderConstant := 0;
	end if;
	HolderExponent := 1;
	return Record('exponent'=HolderExponent,'constant'=HolderConstant,'avoid_roots'=false,'max_degree'=deg-1);
end proc;

# Holder information for exponentials of the form x^alpha
HolderInformationForExponential := proc(function_exponent)
	local output_function;
	output_function := proc(F, PointEstimate, EstimateAccuracy)
		return Record('exponent'=function_exponent,'constant'=1,'avoid_roots'=false,'max_degree'=1);
	end proc;
	return output_function;
end proc;

HolderInformationForRationalPolynomial := proc(P, PointEstimate, EstimateAccuracy,domain_estimate:=false)
    # INPUT: Univariate rational polynomial  of the form F/G

    # OUTPUT: A record of the form [exponent, constant] such that
    # F/G is certifiably Holder with those values
	local F, G, differential_numerator, numerator_constant, max_value,
		  denominator_constant, min_value,FactorsAndMultiplicities,ClassifiedRoots,MINIMUM_NONZERO;
	if domain_estimate then
		FactorsAndMultiplicities := SqrFreeFactorization(denom(simplify(P)));
		ClassifiedRoots := map(EstimateAndClassifySolutions, FactorsAndMultiplicities, EstimateAccuracy);
		ClassifiedRoots := FlattenOnce(ClassifiedRoots);
		return Record('avoid_roots'=[seq(ClassifiedRoots[i]:-value,i=1..nops(ClassifiedRoots))]);
	end if;

	MINIMUM_NONZERO := 3*10^(-10);
	F := numer(simplify(P));
	G := denom(simplify(P));

	if type(F, integer) then
		differential_numerator := -diff(G, indets(G)[1] $ 1)*F;
	elif type(G, integer) then
		differential_numerator := G*diff(F, indets(F)[1] $ 1);
	else differential_numerator := -diff(G, indets(G)[1] $ 1)*F + G*diff(F, indets(F)[1] $ 1);
	end if;
	numerator_constant := HolderInformationForPolynomial(differential_numerator, PointEstimate, EstimateAccuracy):-constant;

	if type(differential_numerator, integer) then
		max_value := abs(differential_numerator) + numerator_constant*EstimateAccuracy;
	else max_value := abs(subs(indets(differential_numerator)[1] = PointEstimate, differential_numerator)) + numerator_constant*EstimateAccuracy;
	end if;
	if type(G, integer) then
		denominator_constant := 1;
	else denominator_constant := HolderInformationForPolynomial(G^2, PointEstimate, EstimateAccuracy):-constant;
	end if;
	if evalb(numerator_constant = denominator_constant) then
		return MINIMUM_NONZERO;
	end if;
	if type(G, integer) then
		min_value := G^2 - denominator_constant*EstimateAccuracy;
	else min_value := abs(subs(indets(G)[1] = PointEstimate, G))^2 - denominator_constant*EstimateAccuracy;
	end if;

	return Record('exponent'=1,'constant'=max_value/min_value,'max_degree'=1);
end proc;

HolderDataToCertificationTolerance := proc(HolderData, ErrorTolerance)
	local HolderConstant, HolderExponent,RoundedDigitsTolerance,RoundedCertNegativeExponent;
	HolderExponent := HolderData:-exponent;
	HolderConstant := HolderData:-constant;
	RoundedDigitsTolerance := max(-floor(log[10](ErrorTolerance^HolderData:-max_degree)),1) + 1;
	if evalf[RoundedDigitsTolerance](HolderConstant) <= 2*10^(-RoundedDigitsTolerance) then
		return 0;
	end if;
	RoundedCertNegativeExponent := floor(-log[10](ErrorTolerance/HolderConstant)/HolderExponent);
	return min(1/10^RoundedCertNegativeExponent, ErrorTolerance, 1);
end proc;

EvaluateFunctionAtRoot := proc(Function, Root, Precision)
	local SymbolicEval,DigitsBeforeDecimalPoint,DigitsAfterDecimalPoint;
	if type(Function,procedure) then
		SymbolicEval := Function(Root);
	else
		SymbolicEval := subs(indets(Function)[1]=Root,Function);
	end if;
	if evalb(SymbolicEval=0) then
		return 0;
	end if;
	# Precision calculation: Figure out necessary number of digits
	# in front of the decimal point, figure out the same for after
	# the decimal point, then add them together.
	DigitsBeforeDecimalPoint := max(ceil(evalf[1](log[10](abs(SymbolicEval)))),1);
	DigitsAfterDecimalPoint := max(-floor(log[10](Precision)),0);
	return evalf[max(1,DigitsBeforeDecimalPoint+DigitsAfterDecimalPoint+1)](SymbolicEval);
end proc;

EstimateRootsAndCertifyEvaluations := proc(PolynomialToSolve, FunctionsToEvaluate, HolderEstimationProcedure, ErrorTolerance, AbandonThreshold := 10^(-100))
	local FactorsAndMultiplicities, Multiplicities, ClassifiedRoots,
	      RootsWithoutAdditionalInformation, HolderBounds, HolderCertificationBounds,
	      SharpenedRootTolerance, ClassifiedRootsWithMultiplicities, WorkingTolerance, ActualTolerance,
	      i, j,arguments_for_output,root_information_for_output,evaluation_information_for_output,current_evals,
	      minimum_gap,current_roots,holder_function,function_roots,estimated_gap_values;
	FactorsAndMultiplicities := SqrFreeFactorization(expand(PolynomialToSolve));
	Multiplicities := [seq(FactorsAndMultiplicities[i][2], i = 1 .. nops(FactorsAndMultiplicities))];
	SharpenedRootTolerance := 0;
	# We need to make sure all estimates are at least 1/10th sharper
	# than the desired number because rounding error in the last reported digit
	# could result in that amount of error
	ActualTolerance := ErrorTolerance - (1/10)*ErrorTolerance;
	WorkingTolerance := max(1/10,ActualTolerance);

	# First Holder information step: Tightening the precision
	# to avoid places where the evaluation functions aren't
	# defined. We need that there are no undefined points
	# with 2*WorkingTolerance of the function to solve's
	# estimated roots
	for i to nops(FunctionsToEvaluate) do
		minimum_gap := 0;
		if type(HolderEstimationProcedure,list) then
			holder_function := HolderEstimationProcedure[i];
		else
			holder_function := HolderEstimationProcedure;
		end if;
		# Note: Minimum gap often contains square roots and Maple doesn't
		# automatically compare expressions with abs in them symbolically.
		# For minimum_gap we can just square, for others we need to evaluate
		# and be careful with precision
		while minimum_gap^2 <= (6*WorkingTolerance)^2 and WorkingTolerance > AbandonThreshold do
			if minimum_gap^2 > 0 then
				WorkingTolerance := min(WorkingTolerance/2,minimum_gap/(6*WorkingTolerance));
			end if;
			function_roots := holder_function(FunctionsToEvaluate[i],1.1,WorkingTolerance,true):-avoid_roots;
			if evalb(function_roots=false) then
				break;
			end if;
			ClassifiedRoots := map(EstimateAndClassifySolutions, FactorsAndMultiplicities, WorkingTolerance);
			ClassifiedRoots := FlattenOnce(ClassifiedRoots);
			current_roots := [seq(ClassifiedRoots[j]:-value, j = 1 .. nops(ClassifiedRoots))];
			estimated_gap_values := FlattenOnce([ seq([seq(abs(function_roots[j] - current_roots[k]),k=1..nops(current_roots))],j=1..nops(function_roots)) ]);
			minimum_gap := min(estimated_gap_values);
			# Need to round down to some rational representation
			# to keep things rational
			minimum_gap := convert(evalf[10](minimum_gap)-2*10^(-10),rational);
		end do
	end do;

	# At this point we've reduced to a good enough tolerance to
	# find local Holder information while dodging points not in the domains
	# of the functions to evaluate. Proceed to find that information, sharpen tolerance,
	# and give final results.
	while SharpenedRootTolerance = 0 and AbandonThreshold < WorkingTolerance do
		ClassifiedRoots := map(EstimateAndClassifySolutions, FactorsAndMultiplicities, WorkingTolerance);
		ClassifiedRoots := FlattenOnce(ClassifiedRoots);
		RootsWithoutAdditionalInformation := [seq(ClassifiedRoots[i]:-value, i = 1 .. nops(ClassifiedRoots))];
		if type(HolderEstimationProcedure, list) then
			if nops(HolderEstimationProcedure) <> nops(FunctionsToEvaluate) then
				error "Number of Holder estimation procedures is different than the number of functions to evaluate.";
			end if;
			HolderBounds := [seq(map[2](HolderEstimationProcedure[i], FunctionsToEvaluate[i], RootsWithoutAdditionalInformation, 2*WorkingTolerance), i = 1 .. nops(FunctionsToEvaluate))];
		else
			HolderBounds := [seq(map[2](HolderEstimationProcedure, FunctionsToEvaluate[i], RootsWithoutAdditionalInformation, 2*WorkingTolerance), i = 1 .. nops(FunctionsToEvaluate))];
		end if;
		HolderBounds := FlattenOnce(HolderBounds);
		HolderCertificationBounds := map(HolderDataToCertificationTolerance, HolderBounds, min(ActualTolerance,WorkingTolerance/2));
		SharpenedRootTolerance := min(HolderCertificationBounds);
		WorkingTolerance := 1/10*WorkingTolerance;
	end do;
	if SharpenedRootTolerance = 0 then
		error "Insufficient precision available to certify evaluations.";
	end if;
	ClassifiedRoots := map(EstimateAndClassifySolutions, FactorsAndMultiplicities, SharpenedRootTolerance);
	ClassifiedRoots := FlattenOnce(ClassifiedRoots);
	arguments_for_output := Record("polynomial"=PolynomialToSolve,"evaluated_functions"=FunctionsToEvaluate,"error_tolerance"=ActualTolerance);
	root_information_for_output := Record("root_values"=[seq(ClassifiedRoots[i]:-value, i=1..nops(ClassifiedRoots))], "root_multiplicities"=Multiplicities);
	evaluation_information_for_output := Record();
	for i to nops(FunctionsToEvaluate) do
		current_evals := Record("evaluations_functions_"||i=map[2](EvaluateFunctionAtRoot,FunctionsToEvaluate[i],root_information_for_output:-root_values,ActualTolerance));
		evaluation_information_for_output := Record[evaluation_information_for_output,current_evals]();
	end do;
	return Record[arguments_for_output,root_information_for_output,evaluation_information_for_output]();
end proc;

end module:


read("est_pi.mpl"):
printf("\nDoing calculation using loop:\n\n"):
est_pi(n):
printf("\nDoing calculation using sum:\n\n"):
native_est_pi(n):


`delta/F3` := (N::posint) -> (x) -> [
 `delta/F`(N)({1,2,3})(1,2)(x),
 `delta/F`(N)({1,2,3})(2,3)(x),
 `delta/F`(N)({1,2,3})(3,1)(x)
];

`Delta/F3` := (N::posint) -> (u) -> [u,u];

`f_plus/F3` := (N::posint) -> proc(vw)
 local v,w;
 v,w := op(vw);
 return [-~ v,v,v -~ w];
end:

`f_minus/F3` := (N::posint) -> proc(vw)
 local v,w;
 v,w := op(vw);
 return [-~ v,v,w -~ v];
end:

`g_plus/F3`  := (N::posint) -> proc(vw)
 local v,w;
 v,w := op(vw);
 return [v,-~ w, -~ v];
end:

`g_minus/F3`  := (N::posint) -> proc(vw)
 local v,w;
 v,w := op(vw);
 return [v,-~ v, -~ w];
end:

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

`is_element/X/F3` := (N::posint) -> proc(xyz)
 global reason;
 local tag,x,y,z;

 tag := "is_element/X/F3";

 if not(type(xyz,list) and nops(xyz) = 3) then
  reason := [tag,"xyz is not a list of length 3",xyz];
  return false;
 fi;

 x,y,z := op(xyz);
 if not(`is_element/R`(N)(x) and
        `is_element/R`(N)(y) and
        `is_element/R`(N)(z)) then
  reason := [tag,"x, y and z are not vectors in R^N",x,y,z];
  return false;
 fi;

 if (x -~ y = [0$N] or y -~ z = [0$N] or z -~ x = [0$N]) then
  reason := [tag,"x, y and z are not distinct",x,y,z];
  return false;
 fi;

 if x +~ y <> [0$N] then
  reason := [tag,"x+y <> 0",x,y];
  return false;
 fi;

 if `norm_2/R`(N)(x) <> 1 or `norm_2/R`(N)(y) <> 1 then
  reason := [tag,"x and y are not normalised",x,y];
  return false;
 fi;

 return true;
end:

`is_equal/X/F3` := (N::posint) -> (xyz0,xyz1) ->
  evalb(simplify(xyz0 -~ xyz1) = [[0$N]$3]);

`is_leq/X/F3` := NULL;
`list_elements/X/F3` := NULL;
`count_elements/X/F3` := NULL;

`random_element/X/F3` := (N::posint) -> proc()
 local x,y,z;
 x := `random_element/sphere`(N-1)();
 y := -~ x;
 z := `random_element/R`(N)();
 while z = x or z = y do
  z := `random_element/R`(N)();
 od;
 return [x,y,z];
end:

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

`is_element/Y/F3` := (N::posint) -> proc(xyz)
 local tag,x,y,z;
 global reason;

 tag := "is_element/Y/F3";

 if not(`is_element/X/F3`(N)(xyz)) then
  reason := [tag,"xyz in not in X",xyz,reason];
  return false;
 fi;

 x,y,z := op(xyz);
 if `d_2/R`(N)(z,x) <> 1 and `d_2/R`(N)(z,y) <> 1 then
  reason := [tag,"z is not at distance 1 from x or y",x,y,z];
  return false;
 fi;

 return true;
end:

`is_equal/Y/F3` := (N::posint) -> (xyz0,xyz1) ->
  evalb(simplify(xyz0 -~ xyz1) = [[0$N]$3]);

`is_leq/Y/F3` := NULL;
`list_elements/Y/F3` := NULL;
`count_elements/Y/F3` := NULL;

`random_element/Y/F3` := (N::posint) -> proc()
 local x,y,w,z;
 x := `random_element/sphere`(N-1)();
 y := -~ x;
 w := `random_element/sphere`(N-1)();
 if rand(0..1)() = 0 then
  z := x +~ w;
 else
  z := y +~ w;
 fi;
 return [x,y,z];
end:

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

`is_element/D_plus/F3` := (N::posint) -> proc(xyz)
 local tag,x,y,z;
 global reason;
 tag := "is_element/D_plus/F3";

 if not(`is_element/X/F3`(N)(xyz)) then
  reason := [tag,"xyz in not in X",xyz,reason];
  return false;
 fi;

 x,y,z := op(xyz);
 if `d_2/R`(N)(z,y) > 1 then
  reason := [tag,"z is not at distance <= 1 from y",x,y,z];
  return false;
 fi;

 return true;
end:

`is_equal/D_plus/F3` := (N::posint) -> (xyz0,xyz1) ->
  evalb(simplify(xyz0 -~ xyz1) = [[0$N]$3]);

`is_leq/D_plus/F3` := NULL;
`list_elements/D_plus/F3` := NULL;
`count_elements/D_plus/F3` := NULL;

`random_element/D_plus/F3` := (N::posint) -> proc()
 local x,y,w,z;
 x := `random_element/sphere`(N-1)();
 y := -~ x;

 if rand(0..5)() = 0 then
  z := [0$N];
 else
  w := `random_element/sphere`(N-1)() *~ rand(1..5)()/5;
  z := y +~ w;
 fi;
 return [x,y,z];
end:

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

`is_element/D_minus/F3` := (N::posint) -> proc(xyz)
 local tag,x,y,z;
 global reason;

 tag := "is_element/D_minus/F3";

 if not(`is_element/X/F3`(N)(xyz)) then
  reason := [tag,"xyz in not in X",xyz,reason];
  return false;
 fi;

 x,y,z := op(xyz);
 if `d_2/R`(N)(z,x) > 1 then
  reason := [tag,"z is not at distance <= 1 from x",x,y,z];
  return false;
 fi;

 return true;
end:

`is_equal/D_minus/F3` := (N::posint) -> (xyz0,xyz1) ->
  evalb(simplify(xyz0 -~ xyz1) = [[0$N]$3]);

`is_leq/D_minus/F3` := NULL;
`list_elements/D_minus/F3` := NULL;
`count_elements/D_minus/F3` := NULL;

`random_element/D_minus/F3` := (N::posint) -> proc()
 local x,y,w,z;
 x := `random_element/sphere`(N-1)();
 y := -~ x;

 if rand(0..5)() = 0 then
  z := [0$N];
 else
  w := `random_element/sphere`(N-1)() *~ rand(1..5)()/5;
  z := x +~ w;
 fi;
 return [x,y,z];
end:

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

`is_element/D_zero/F3` := (N::posint) -> proc(xyz)
 local tag,x,y,z;
 global reason;

 tag := "is_element/D_zero/F3";

 if not(`is_element/X/F3`(N)(xyz)) then
  reason := [tag,"xyz in not in X",xyz,reason];
  return false;
 fi;

 x,y,z := op(xyz);
 if `d_2/R`(N)(z,x)^2 < 1 or `d_2/R`(N)(z,y)^2 < 1 then
  reason := [tag,"z is not at distance >= 1 from x and y",x,y,z];
  return false;
 fi;

 return true;
end:

`is_equal/D_zero/F3` := (N::posint) -> (xyz0,xyz1) ->
  evalb(simplify(xyz0 -~ xyz1) = [[0$N]$3]);

`is_leq/D_zero/F3` := NULL;
`list_elements/D_zero/F3` := NULL;
`count_elements/D_zero/F3` := NULL;

`random_element/D_zero/F3` := (N::posint) -> proc()
 local x,y,w,z,r,i;
 x := `random_element/sphere`(N-1)();
 y := -~ x;

 z := x;
 r := 0;
 while r < 1 do
  if rand(0..5)() = 0 then
   w := `random_element/sphere`(N-1)();
   if rand(0..1)() = 0 then
    z := x +~ w;
   else
    z := y +~ w;
   fi;
  else
   z := `random_element/sphere`(N-1)(100) *~ (rand(1..300)()/100);
  fi;
  r := min(add((x[i]-z[i])^2,i=1..N),add((y[i]-z[i])^2,i=1..N));
 od:

 return [x,y,z];
end:

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

`q_plus/F3` := (N::posint) -> proc(xyz)
 local x,y,z;
 x,y,z := op(xyz);
 return y +~ (z -~ y) /~ `norm_2/R`(N)(z -~ y);
end:

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

`q_minus/F3` := (N::posint) -> proc(xyz)
 local x,y,z;
 x,y,z := op(xyz);
 return y +~ (z -~ x) /~ `norm_2/R`(N)(z -~ x);
end:

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

`q_zero/F3` := (N::posint) -> proc(xyz)
 local x,y,z;
 x,y,z := op(xyz);
 if z = [0$N] then
  return [0$N];
 else
  return (2*`dot/R`(N)(y,z)/`norm_2/R`(N)(z)^2) *~ z;
 fi;
end:

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

`q/F3` := (N::posint) -> proc(xyz)
 local x,y,z;
 x,y,z := op(xyz);
 if   `is_element/D_plus/F3`(N)(xyz) then
  return `q_plus/F3`(N)(xyz);
 elif `is_element/D_minus/F3`(N)(xyz) then
  return `q_minus/F3`(N)(xyz);
 else
  return `q_zero/F3`(N)(xyz);
 fi;
end:

`r/F3` := (N::posint) -> proc(xyz)
 local x,y,z,x0,y0,z0,n0;
 x,y,z := op(xyz);

 x0 := (x -~ y)/~2;
 n0 := `norm_2/R`(N)(x0);
 x0 := x0 /~ n0;
 y0 := -~ x0;
 z0 := (z -~ ((x +~ y)/~2))/~ n0;
 return [x0,y0,`q/F3`(N)([x0,y0,z0])];
end:


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

`eta/stasheff_star` := proc(A::set)
 if nops(A) <> 1 then return FAIL; fi;
 return [[op(A)],table([A = 1])];
end;

`gamma/stasheff_star` := (A::set,B::set) -> (p) -> proc(U,V)
 local R,S,T,r,s,t,b,n,JJ,J,pJ,J1,i,j,k;

 S,s := op(U);
 R := table();
 r := table();
 for b in B do
  R[b],r[b] := op(V[b]);
 od;

 T := [seq(op(R[b]),b in S)];

 t := table();

 n := nops(T);
 JJ := {seq(seq({seq(T[k],k=i..j)},j=i..n),i=1..n)};
 for J in JJ do
  pJ := map(j -> p[j],J);
  J1 := select(j -> member(p[j],pJ),A);
  if J = J1 then
   t[J] := s[pJ];
  elif nops(pJ) = 1 then
   t[J] := r[op(pJ)][J];
  else
   t[J] := 0;
  fi;
 od:

 return [T,eval(t)];
end;


 # a function returning (i + j) * -998
 func $foo (
# var %i xxx,
  var %i i32,
   var %j i32,
   var %k i32) i32 {
   return (
     select i32 (
      dread i32 %i,
      dread i32 %j,
      dread i32 %k))}

 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


  ReparamDetermined := proc(lo :: LO(name, anything), kb::t_kb)
    local h;
    h := op(1,lo);
    LO(h,
       evalindets(op(2,lo),
                  'And'('specfunc({Int,int})',
                        'anyfunc'(anything, 'name=anything')),
                  g -> `if`(determined(op(1,g),h), reparam(g,h,kb), g)))
  end proc;

  determined := proc(e, h :: name)
    local i;
    for i in indets(e, 'specfunc({Int,int})') do
      if hastype(IntegrationTools:-GetIntegrand(i),
           'applyintegrand'('identical'(h),
             'dependent'(IntegrationTools:-GetVariable(i)))) then
        return false
      end if
    end do;
    return true
  end proc;

  #Beginning of Carl's code devoted to disintegration and the reparametrization (aka change
  #of variables) of integrals and sums.

  #Finds the innermost Ints or Sums in an expression, that is, those which
  #don't contain further Ints or Sums
  innermostIntSum:= proc(e::anything, $)::set(specfunc({Int,Sum}));
    select(IS-> nops(indets(IS, specfunc({Int,Sum}))) = 1, indets(e, specfunc({Int,Sum})))
  end proc;

  #my substitute for IntegrationTools:-Change
  ChangeVarInt:= proc(J::specfunc(Int), target::algebraic, $)
    local
    newJ, #What J will become.
    x::name:= op([2,1], J),
    u::name:= gensym(x),    #new variable of integration
    s,                      #What x will become
    F                       #Boolean: flip integral?
    ;
    s:= {solve({u = target}, {x})};
    if nops(s) = 0 then
      userinfo(1, 'reparam', "Can't solve substitution target.");
      return J
    end if;
    if nops(s) > 1 then
      userinfo(1, 'reparam', "Case of multi-branched substitution not handled.");
      return J
    end if;
    s:= s[];

    newJ:= Int(
      eval(implicitdiff(u = target, x, u)*op(1,J), s),
      u=
      limit(target, x= op([2,2,1], J), right).. #lower limit
      limit(target, x= op([2,2,2], J), left),   #upper limit
      op(3.., J) #optional Int args (unlikely to be used)
      );

    #If lower limit > upper limit, then flip limits of integration.
    F:= is(op([2,2,1], newJ) > op([2,2,2], newJ));
    if F::truefalse then
      if F then
        userinfo(2, reparam, "Switching limits:", op([2,2], newJ));
        newJ:= IntegrationTools:-Flip(newJ)
      end if
    else #If inequality can't be decided, then don't reverse.
      userinfo(1, reparam, "Can't order new limits:", op([2,2], newJ))
    end if;
    newJ
  end proc;

  #main procedure for int/sum reparamterizations
  reparam:= proc(e::algebraic, h::symbol, ctx::{list,t_kb}:= [], $)
    local
    J:= innermostIntSum(e),   #the integral or sum
    newJ, #What J will become possible subs target(s)
    oldarg:=
    map2(op, 2, indets(e, specfunc(applyintegrand))),
    kb := build_kb(ctx,"reparam"),
    newarg, Jbnds, del
    ;
    if not(oldargs::{algebraic, set(algebraic), list(algebraic)}) then
      userinfo(2, 'procname', "Unexpected arguements to applyintegrand");
      return e;
    end if;
    if nops(J) = 0 then
      userinfo(2, 'procname', "No sum or integral found.");
      return e
    end if;
    if nops(J) > 1 then
      userinfo(1, 'procname', "Case of multiple innermost Int or Sums not yet handled.");
      return e
    end if;
    J:= J[];
    if J::specfunc(Sum) then
      userinfo(1, 'procname', "Sums not yet handled.");
      return e
    end if;
    if hastype(op(1,J), t_pw_or_part) then
      userinfo(1, 'procname', "Reparam of piecewise body not yet handled.");
      return e;
    end if;
    if nops(oldarg) <> 1 then
      userinfo(1, 'procname', "More than 1 reparam possible:", oldarg);
      return e
    end if;
    oldarg:= oldarg[];   #Extract the reparam target.

    Jbnds := op([2,2], J);
    del := simplify_assuming(op(2,Jbnds)-op(1,Jbnds), kb);
    if del in {-1,1} then
      userinfo(2, 'procname', "No need for a reparameterization (bound diff is 1).");
      return e;
    end if;
    if has(Jbnds, infinity) then
      userinfo(2, 'procname', "Reparam of infinity bounds probably won't work.");
      return e;
    end if;

    # if the arg to applyintegrand is not a variable or a datum or an index
    # into an array
    if oldarg::And(Not({symbol, function, indexed, list})) then
      newarg := oldarg;
    # if the bounds don't contain infinity, do not have a difference of 1
    # and contain a product of that variable
    elif oldarg::symbol and
         op(1,J)::`*`   and
         select(type, [op(op(1,J))], identical(oldarg))<>[]
    then
      newarg := oldarg/del;
    else
      userinfo(2, 'procname', "No need for a reparameterization.");
      return e
    end if;

    #Make the change of vars.
    newJ:= kb_assuming_mb(ChangeVarInt@op, [J, newarg], kb, _->FAIL);

    if newJ = 0 or not(newJ::specfunc(Int)) or newJ=FAIL then
      userinfo(
        1, 'procname',
        printf(
          "Invalid reparam, likely due to improper handling of an infinity issue.\n"
          "u subs:%a",
          #Reformat the ChangeVarInt command for readability.
          subs(
            x= ':-x',
            'ChangeVarInt'(
              subsindets(
                J,
                   specfunc(applyintegrand),
                f-> ':-h'(op(2,f)))),
            newarg)));
      return e;
    end if;
    subs(J= newJ, e)
  end proc;


InvClassify:=module()
    option  package;
    export  doClassify,                     # 进行分类
            InvSol,                         # 解对象
            RepSol;                         # 代表元对象
    local   ModuleLoad;

$include "headers.mpl"

$include "InvSol.mpl"
$include "RepSol.mpl"

$include "Basic.mpl"
$include "Classifyer.mpl"
$include "Combine.mpl"
$include "Condition.mpl"
$include "Fetch.mpl"
$include "InvOrder.mpl"
$include "Interaction.mpl"
$include "InvSimplify.mpl"
$include "Logout.mpl"
$include "Utils.mpl"

    ModuleLoad:=proc()
        PDETools:-declare(quiet):
    end proc:
    ModuleLoad();

    # 进行分类
    doClassify:=proc(vv::list)
        local As,A,eqs,sols,reps;
        As,A,eqs:=getTransMatAndPDE(vv);
        classify(A,As,eqs);
        sols:=getSols();
        reps:=buildReps(sols);
        return reps;
    end proc:

end module:


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: work_mgga_c *)
(* prefix:
  mgga_c_m05_params *params;

  assert(p->params != NULL);
  params = (mgga_c_m05_params * )(p->params);
*)

$define lda_c_pw_params
$define lda_c_pw_modified_params
$include "lda_c_pw.mpl"

$include "b97.mpl"

m05_comp := (rs, z, spin, xs, ts) ->
  + lda_stoll_par(f_pw, rs,  z,  1)
  * b97_g(params_a_gamma_ss, params_a_css, xs)
  * Fermi_D(xs, ts):

(* The parallel and perpendicular components of the energy *)
m05_fpar  := (rs, z, xs0, xs1, ts0, ts1) ->
  + m05_comp(rs,  z,  1, xs0, ts0)
  + m05_comp(rs, -z, -1, xs1, ts1):

m05_fperp := (rs, z, xs0, xs1, ts0, ts1) ->
  + lda_stoll_perp(f_pw, rs,  z)
  * b97_g(params_a_gamma_ab, params_a_cab, sqrt(xs0^2 + xs1^2)):

f_m05 := (rs, z, xs0, xs1, ts0, ts1) ->
  + m05_fpar (rs, z, xs0, xs1, ts0, ts1)
  + m05_fperp(rs, z, xs0, xs1, ts0, ts1):

f := (rs, z, xt, xs0, xs1, ts0, ts1, us0, us1) ->
  f_m05(rs, z, xs0, xs1, ts0, ts1):


  #Disintegration *
  #---------------*
  #Abbreviations:
  #     wrt = "with respect to".
  #     wrt var = "a variable wrt which disintegration is performed". There may be
  #          more than one of these, in which case they are passed (in the 2nd arg)
  #          as Pairs, possibly nested.
  #     wrt-var type: Each wrt var may be continuous (Lebesgue), discrete (Counting),
  #          point evaluation (Dirac), and there may be other types added later.
  disint:= module()
  export ModuleApply, `do`;
  local
    #Dispatch table for wrt-var types. For each, there is a "cond constructor",
    #a "disintegrator", and a "disintegrator_arg_extractor". The cond constructor
    #builds the associated relation in the master piecewise; the disintegrator
    #does the differentiation or whatever operator is analogous to differentiation
    #for that type after the measure has been `improve`d; and the
    #disintegrator_arg_extractor builds the 2nd arg to the disintegrator from
    #the t_disint_var passed in the 2nd arg of disint.
    #
    #APIs:
    #     Indices:
    #          Must be the op(0, ...) of the expression passed with the
    #          wrt var.
    #     cond_constructor: (algebraic,name)-> boolean (most likely, a relation)
    #     disintegrator: (algebraic, {name, name=anything})-> algebraic
    #     disintegrator_arg_extractor:
    #          (`&M`(name, t_wrt_var_type))-> {name, name= anything}
    Wrt_var_types::static:= table([
         Lebesgue= Record(
              cond_constructor= `<=`,
              disintegrator= diff,
              disintegrator_arg_extractor= (A-> op(1,A)),
              disintegrator_type_extractor= [ (ll-> kb-> KB:-genLebesgue(op(1,ll), op(op(2,ll)), kb)), AlmostEveryReal ]
         ),
         Counting= Record(
              cond_constructor= `<=`,
              disintegrator= LREtools[delta],
              disintegrator_arg_extractor= (A-> op(1,A)),
              disintegrator_type_extractor= [ (ll-> kb-> KB:-genSummation(op(1,ll), op(op(2,ll)), kb)), HInt ]
         ),
         #Ret is aka Dirac.
         Ret= Record(
              cond_constructor= `=`,
              disintegrator='eval',
              disintegrator_arg_extractor= (A-> op(1,A)= op([2,1], A)),
              #E.g., (x &M Ret(3)) --> (x= 3).

              disintegrator_type_extractor= [ (ll -> kb -> (op(1,ll), kb)) ]
              # disintegrator_type_extractor= (ll-> kb-> KB:-genLet(op(1,ll), op([2,1],ll), kb))
         )
    ]),

    #types for disint wrt vars (2nd arg to disint)
    t_wrt_var_type,
    t_disint_var, #Curiosity: giving this a type `type` causes a kernel crash
                  #during update-archive.
    t_disint_var_pair,
    ModuleLoad::static:= proc($) #Needed to declare types.
         TypeTools:-AddType(
              t_wrt_var_type,
              satisfies(t-> assigned(Wrt_var_types[op(0,t)]))
         );
         TypeTools:-AddType(t_disint_var, {name, name &M t_wrt_var_type});
         TypeTools:-AddType(     #Caution: recursive type: Make sure base cases
           t_disint_var_pair, #are on left (a la McCarthy rule).
              'Pair'(Or(t_disint_var, t_disint_var_pair) $ 2)
         )
    end proc,
    #end of types for disint

    DV::table,  #wrt vars, with their types and conditions
    p::symbol,  #"pair"--abstract representation
    #`path` is layers of fst(...) and snd(...) built by traversing tree
    #(Weird Maple syntax note: Module prefixes seem to be required for
    #assertion type checking. Failure to include them causes kernel crash
    #during execution.)
    path::{specfunc({Hakaru:-fst, Hakaru:-snd}), symbol},

    #Parses the 2nd arg---the wrt vars.
    # works by side-effect: accumulates "paths" to variables in T
    # via the module variable DV.
    traverse_var_tree::static:= proc(
         T::{t_disint_var, t_disint_var_pair}, $
    )::identical(NULL);
    local
         v::name, #the wrt var
         M::NewSLO:-disint:-t_wrt_var_type,
         pp, #iterator over [fst, snd]---the deconstructors of Pair
         vM, vK, mks, mk_ty, mk_ctx
    ;
         if T::t_disint_var then
              #Add a default wrt-var type if none appears.
              (v,M):= op(`if`(T::name, T &M 'Lebesgue'((-1,1)*~infinity), T));
              vK   := op(0,M);
              vM   := v &M M;
              mks := Wrt_var_types[vK]:-disintegrator_type_extractor;
              mk_ty := op(1,mks);
              if nops(mks)=2 then mk_ctx := ((e)->lam(v,op(2,mks)(),e));
              else                mk_ctx := ((e)->e);
              end if;
              DV[v]:= Record(
                   'wrt_var_type'= M,
                   'path'= path,
                   'disintegrator_arg'=
                        Wrt_var_types[vK]:-disintegrator_arg_extractor(vM),
                   'disintegrator_mktype'=mk_ty(vM),
                   'disintegrator_mkctx' =mk_ctx
              );
              path:= op(path) #Peel off outer layer: fst or snd.
         else #T::Pair(..., ...)---deconstruct recursively.
              for pp in [fst, snd] do path:= pp(path); thisproc(pp(T)) end do
         end if;
         NULL
    end proc, #disint:-traverse_var_tree

    subs_disint_data := proc(sub_vars, expr, $)
        local V:= [indices(DV, 'nolist')], v
            , vars := [indices(sub_vars, 'nolist')]
            , eqns := [seq(v=sub_vars[v], v=vars)]
            , sb := (x->subs(eqns,x));

        for v in V do
            DV[v]:-disintegrator_arg := sb(DV[v]:-disintegrator_arg);
        end do;

        sb(expr);
    end proc;

  ;
    ModuleApply := proc()::t_Hakaru;
      local todo,expr,t;
      expr, todo := disint:-`do`(args)[];
      for t in todo do expr := t(expr); end do; expr;
    end proc;

   `do` := proc(
    m::t_Hakaru,
    #var &M wrt-var type, or Pairs thereof
    A::{t_disint_var, t_disint_var_pair},
    ctx::t_kb_atoms:= [] #context: parameter assumptions, "knowledge"
   ):: [t_Hakaru, list(appliable)];
   local
    mc,  #final integral to be passed to improve @ toLO; then result
         #of each disintegration step
    kb, var_rn := table(), mc_prts,
    V, #wrt vars
    v::name, #iterator over V
    improve_opts := [],
    todo := NULL,
    atodo := proc(z) todo := eval(todo), z; end proc,
    di, da
   ;
    if not {_rest} in {{'do_lam'},{}} then
      error "bad extra args: %1", {_rest};
    end if;

    #Init module variables.
    DV:= table();
    p:= gensym('p');
    path:= fst(p);

    traverse_var_tree(A); # collect information about A in DV
    V:= [indices(DV, 'nolist')];

    mc:= Bind(
         m, p,
         #The piecewise condition is a conjunction of conditions, each
         #condition formed from a (var,path) pair from DV.
         piecewise(
          And(seq(
               Wrt_var_types[op(0, DV[v]:-wrt_var_type)]:-
                cond_constructor(DV[v]:-path, v),
               v= V
          )),
          Ret(snd(p)),
          Msum()
         ));

    kb := empty;
    for v in ListTools[Reverse](V) do
        var_rn[v], kb := DV[v]:-disintegrator_mktype(kb);
    end do;
    kb := build_kb(ctx,"disint",kb);
    mc := subs_disint_data(var_rn, mc);

    atodo(x->toLO(x));
    atodo(x->improve(x, _ctx=kb,improve_opts));
    #Does the order of application of the disintegrators matter?
    #Theoretically, I think not, it's just like differentiation. As far
    #as Maple's ability to do the computation, maybe it matters.
    for v in V do
      di   := Wrt_var_types[op(0, DV[v]:-wrt_var_type)]:-disintegrator;
      da   := DV[v]:-disintegrator_arg;
      atodo(x->applyop(di, 2, x, da));
      atodo(x->improve(x, _ctx=kb,improve_opts));
    end do;

    atodo(x->fromLO(x, _ctx= kb));
    ## simplify integrals which do not mention the LO var (i.e. integrals in
    ## weights). this is a hack, we should do this inside of `improve'* in
    ## the correct place.  It is required to see the correct output for
    ## d7_normalFB1.
    #
    ## * Or, some other simplifier, which does only specific things - note
    ## the simplification we hope to see here (in d7) can only be done after
    ## the application of the `diff'. So just chucking it into `improve'
    ## might not be the correct thing to do. Calling `improve' at all might
    ## be wrong since we roundtrip through pulling off domains and replacing
    ## them (this is sort of expensive..), although we may also want such
    ## simplifications here - the application of the `diff' might give a new
    ## domain problem that can be improved significanly.
    # 'Simplify' partitions in weights by converting them to piecewise and
    # letting piecewise simplify do the work; and evaluate integrals in weights
    atodo(x->
          subsindets( x, 'Weight(anything, anything)'
                    , mw -> applyop(x->simplify_assuming(
                        subsindets( subsindets(x,Partition,PartitionToPW)
                                  , And(specfunc(`Int`)
                                       ,satisfies(e->indets(e,specfunc(`exp`))<>{})
                                       )
                                  , value
                                  )
                        , kb ), 1, mw)));

    atodo(x->subs_disint_data( table([seq(var_rn[v]=v, v=V)]), x));

    if 'do_lam' in {_rest} then
      for v in ListTools[Reverse](V) do
        atodo(x->DV[v]:-disintegrator_mkctx(x));
      end do;
    end if;

    [mc, [todo]];
   end proc; #disint:-ModuleApply
   ModuleLoad();
  end module; #disint
  ###################### end of Carl's code ######################


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)

# Note that the files have to be included in this specific order
$define gga_x_pbe_params
$include "gga_x_pbe.mpl"

$include "op.mpl"

op_qab         := 2.3789:
op_enhancement := xs -> pbe_f(xs):


# simplicial_interval(n) is just the set [n] = {0,1,...,n}

`is_element/simplicial_interval` := (n::nonnegint) -> proc(k)
 type(k,nonnegint) and k <= n;
end:

`is_equal/simplicial_interval` := (n::nonnegint) -> (a,b) -> evalb(a = b);

`is_leq/simplicial_interval` := (n::nonnegint) -> (a,b) -> evalb(a <= b);

`list_elements/simplicial_interval` := proc(n::nonnegint)
 local i;
 [seq(i,i=0..n)];
end:

`count_elements/simplicial_interval` := (n::nonnegint) -> n+1;

`random_element/simplicial_interval` := (n::nonnegint) -> proc()
 rand(0..n)();
end:

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

`is_element/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f)
 global reason;
 local N,M,i;

 N := {seq(i,i=0..n)};
 M := {seq(i,i=0..m)};

 if not(`is_element/maps`(N,M)(f)) then
  reason := [convert(procname,string),"not a map from [n] to [m]",reason];
  return false;
 fi;

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

 return true;
end:

`to_list/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f)
 local i;
 return [seq(f[i],i=0..n)];
end:

`is_equal/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f,g)
 evalb(`to_list/simplicial_maps`(n,m)(f) = `to_list/simplicial_maps`(n,m)(g));
end:

`is_leq/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f,g)
 local u;
 u := `to_list/simplicial_maps`(n,m)(g) -~ `to_list/simplicial_maps`(n,m)(f);
 evalb( min(op(u)) >= 0 );
end:

# This assumes that
# *  p is a subset of {1,...,n} of size k
# *  q is a subset of {0,...,m} of size k+1
# It returns the nondecreasing map f : {0,...,n} -> {0,...,m} such that
# *  The image of f is q
# * { i > 0 : f(i) > f(i-1) } = p.

`build/simplicial_maps` :=  (n::nonnegint,m::nonnegint) -> proc(k,p,q)
 local pp,i,j,f;
 pp := [0,op(p),n+1];
 f := table();
 i := 0;
 for i from 0 to k do
  for j from pp[i+1] to pp[i+2]-1 do
   f[j] := q[i+1];
  od;
 od;

 return eval(f);
end:

`random_element/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc()
 local p,q,i,j,k,l,f;

 k := rand(0..min(n,m))();
 p := combinat[randcomb](n,k);
 q := combinat[randcomb](m+1,k+1) -~ 1;
 return `build/simplicial_maps`(n,m)(k,p,q);
end:

`list_elements/simplicial_maps` := proc(n::nonnegint,m::nonnegint)
 option remember;
 local i;

 if n = 0 then
  return [seq(`C/simplicial_maps`(0,i),i=0..m)];
 else
  if m = 0 then
   return [`C/simplicial_maps`(n,0)];
  else
   return [op(map(`I/simplicial_maps`(n,m-1),`list_elements/simplicial_maps`(n,m-1))),
           op(map(`T/simplicial_maps`(n-1,m),`list_elements/simplicial_maps`(n-1,m)))];
  fi;
 fi;
end:

`count_elements/simplicial_maps` := (n::nonnegint,m::nonnegint) -> binomial(n+m+1,m);

# Constant maps
`C/simplicial_maps` := proc(n::nonnegint,k::nonnegint)
 local i;
 table([seq(i=k,i=0..n)]);
end:

# Inclusion Delta(n,m) -> Delta(n,m+1)
`I/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f)
 local i;
 table([seq(i=f[i],i=0..n)]);
end:

# Map Delta(n,m) -> Delta(n+1,m): extend by sending n+1 to m
`T/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f)
 local i;
 table([seq(i=f[i],i=0..n),n+1=m]);
end:

# `delta/simplicial_maps`(n)(i) : [n] >-> [n+1]; image omits i
`delta/simplicial_maps` := proc(n::nonnegint,i::nonnegint)
 local j;
 if i > n+1 then return FAIL; fi;
 return table([seq(j=j,j=0..i-1),seq(j=j+1,j=i..n)]);
end:

# `sigma/simplicial_maps`(n)(i) : [n] ->> [n-1]; takes the value i twice
`sigma/simplicial_maps` := proc(n::nonnegint,i::nonnegint)
 local j;
 if i > n-1 then return FAIL; fi;
 return table([seq(j=j,j=0..i),seq(j=j-1,j=i+1..n)]);
end:

`id/simplicial_maps` := proc(n::nonnegint)
 local i;
 return table([seq(i=i,i=0..n)]);
end:

# `compose/simplicial_maps`(n,m,p)(f,g) assumes that f:[n] -> [m] and g:[m] -> [p],
# and it returns the composite g o f : [n] -> [p].

`compose/simplicial_maps` := (n::nonnegint,m::nonnegint,p::nonnegint) -> proc(f,g)
 local i;
 table([seq(i = g[f[i]],i=0..n)]);
end:

`is_wide/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f)
 return evalb(f[0] = 0 and f[n] = m);
end:

`is_mono/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f)
 local i;

 if m < n then return false; fi;
 for i from 0 to n-1 do
  if f[i] = f[i+1] then return false; fi;
 od;
 return true;
end:

`is_epi/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f)
 local i,j;

 if n < m then return false; fi;
 if f[0] <> 0 then return false; fi;
 if f[n] <> m then return false; fi;

 j := 0;
 for i from 1 to n do
  if (f[i] <> j and f[i] <> j+1) then
   return false;
  fi;
  j := f[i];
 od;
 return true;
end:

# This assumes that f : [n] -> [m], and it returns [k,g,h] such that
# g : [n] ->> [k] and h : [k] >-> [m] and f = h o g

`factor/simplicial_maps` := (n::nonnegint,m::nonnegint) -> proc(f)
 local K,k,g,h,hi,i;

 K := sort([op({seq(f[i],i=0..n)})]);
 k := nops(K) - 1;
 h := table([seq(i=K[i+1],i=0..k)]);
 hi := table();
 for i from 0 to k do hi[h[i]] := i; od;
 g := table([seq(i = hi[f[i]],i=0..n)]);
 return [k,eval(g),eval(h)];
end:

`describe/simplicial_maps` := (n::nonnegint) -> proc(f)
 local i;
 cat(seq(nat_code[f[i]],i=0..n));
end:

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

`is_element/simplicial_epi` := (n::nonnegint,m::nonnegint) -> proc(f)
 global reason;

 if not(`is_element/simplicial_maps`(n,m)(f)) then
  reason := [convert(procname,string),"not a simplicial map from [n] to [m]",reason];
  return false;
 fi;

 return `is_epi/simplicial_maps`(n,m)(f);
end:

`is_equal/simplicial_epi` :=  (n::nonnegint,m::nonnegint) -> proc(f,g)
 `is_equal/simplicial_maps`(n,m)(f,g);
end:

`is_leq/simplicial_epi` :=  (n::nonnegint,m::nonnegint) -> proc(f,g)
 `is_leq/simplicial_maps`(n,m)(f,g);
end:

`random_element/simplicial_epi` := (n::nonnegint,m::nonnegint) -> proc()
 local p,q,k,f,i;

 if n < m then return FAIL; fi;

 k := m;
 p := combinat[randcomb](n,k);
 q := [seq(i,i=0..m)];
 return `build/simplicial_maps`(n,m)(k,p,q);
end:

`list_elements/simplicial_epi` := proc(n::nonnegint,m::nonnegint)
 local P,L,S,f,i,j;

 P := combinat[choose]([seq(i,i=1..n)],m);
 L := [];
 for S in P do
  f := table();
  for i from 0 to S[1]-1 do f[i] := 0; od;
  for j from 1 to m-1 do
   for i from S[j] to S[j+1]-1 do f[i] := j; od;
  od;
  for i from S[m] to n do f[i] := m; od;
  L := [op(L),eval(f)];
 od:

 return L;
end:

`count_elements/simplicial_epi` := (n::nonnegint,m::nonnegint) -> binomial(n,m);

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

`is_element/simplicial_mono` := (n::nonnegint,m::nonnegint) -> proc(f)
 global reason;

 if not(`is_element/simplicial_maps`(n,m)(f)) then
  reason := [convert(procname,string),"not a simplicial map from [n] to [m]",reason];
  return false;
 fi;

 return `is_mono/simplicial_maps`(n,m)(f);
end:

`is_equal/simplicial_mono` :=  (n::nonnegint,m::nonnegint) -> proc(f,g)
 `is_equal/simplicial_maps`(n,m)(f,g);
end:

`is_leq/simplicial_mono` :=  (n::nonnegint,m::nonnegint) -> proc(f,g)
 `is_leq/simplicial_maps`(n,m)(f,g);
end:

`random_element/simplicial_mono` := (n::nonnegint,m::nonnegint) -> proc()
 local p,q,i,j,k,l,f;

 if n > m then return FAIL; fi;

 k := n;
 p := [seq(i,i=1..n)];
 q := combinat[randcomb](m+1,k+1) -~ 1;
 return `build/simplicial_maps`(n,m)(k,p,q);
end:

`list_elements/simplicial_mono` := proc(n::nonnegint,m::nonnegint)
 local P,L,S,f,i,j;

 P := combinat[choose]([seq(i,i=0..m)],n+1);
 L := [];
 for S in P do
  f := table();
  for i from 0 to n do f[i] := S[i+1]; od;
  L := [op(L),eval(f)];
 od:

 return L;
end:

`count_elements/simplicial_mono` := (n::nonnegint,m::nonnegint) -> binomial(m+1,n+1);


(*
    交换子表计算：
    + 计算交换子的表达式形式。
    + 将交换子用生成元进行线性表出。
    + 计算伴随变换矩阵。
    + 生成偏微分方程组，并求解不变量。
*)
$ifndef _BASIC_
$define _BASIC_

$include "Logout.mpl"
$include "InvSimplify.mpl"

$ifndef _HEADERS_
PDETools:-declare(quiet):
macro(Pa=`\x26\x50\x61\x72\x74\x69\x61\x6C\x44\x3B`);# 偏导符号作为函数名
$endif

SymsHolder:=module()
    option object;
    export  default_syms:={x,y,z,t,u,v,w},      # 默认符号集
            syms:=default_syms,                 # 当前符号集
            pnames:={a,c,d,epsilon,Delta,phi};  # 受保护的名字
end module:

# 修改微分算子的符号集合
setSymbols:=proc(s::set(name):=SymsHolder:-default_syms)
    description "设置函数的变量名集合";
    if ( s intersect SymsHolder:-pnames <> {}) then
        error "变量不能包含%1",SymsHolder:-pnames;
    end if;
    SymsHolder:-syms:=s;
end proc;

# 获取变量名集合
getSymbols:=proc()
    description "获取变量名集合";
    SymsHolder:-syms;
end proc;

# 自定义微分算子操作，作用到函数f上
d:=proc()
    description "用于生成微分算子表达式";
    if not {_passed} subset SymsHolder:-syms then
        error "表达式只能包含以下变量 %1, 可以使用 setSymbols 设置变量集合，但是不能包含 %2",SymsHolder:-syms,SymsHolder:-pnames;
    end if;
    return diff(Pa(SymsHolder:-syms[]),_passed);
end proc;

# 自定义交换子计算符
`&c`:=proc(a,b)
    description "计算两个生成微分算子的交换子";
    expand(eval(subs(Pa(SymsHolder:-syms[])=b,a)-subs(Pa(SymsHolder:-syms[])=a,b)));
end proc:

(*
* 将表达式分解为非线性项并提取系数
* 输入：
*      f 表达式
* 输出：
*     T 非线性项->系数 的映射表
*)
getKd:=proc(f)
    local T:=table(),kd;

    (*
    * 将表达式分解为非线性基并提取系数的递归子函数
    * 输入：
    *     f 表达式
    *     T 保存结果的表
    * 输出：
    *     T T会被修改
    *)
    kd:=proc(f,T)
    local i,p,v,x;
    if type(f,`+`) then
        for i from 1 to nops(f) do
            thisproc(op(i,f),T);
        end do;
    elif type(f,`*`) then
        p:=1;
        v:=1;
        for i from 1 to nops(f) do
            x:=op(i,f);
            if type(x,extended_numeric) then
                p:=p*x;
            else
                v:=v*x;
            end if;
        end do;
        T[v]:=p;
    else
        T[f]:=1;
    end if;
    return;
    end proc:

    kd(f,T);
    return eval(T);
end proc:

(*
* 获取表达式关于给定非线性项集的系数向量
* 输入：
*     f    表达式
*     s    非线性项集
* 输出：
*     v    系数向量
*)
getPmVec:=proc(f,s)
    local n,v,i,tb;
    tb:=getKd(f);
    n:=numelems(s);
    v:=Vector(n);
    for i from 1 to n do
        if assigned(tb[s[i]]) then
            v[i]:=tb[s[i]];
        end if;
    end do;
    return eval(v);
end proc:

(*
* 求解表达式关于基的线性表出
* 输入：
*     f    表达式
*     A    基关于非线性项集的系数矩阵
*     stbs    非线性项集
*     sbs    基的符号表示
* 输出：
*     r    表达式关于基的线性表出，求解失败返回原表达式
*)
ans2v:=proc(f,A,stbs)
    local r;
    try
        r:=LinearAlgebra[LinearSolve](A,getPmVec(f,stbs));
    catch:
        error "所给生成元不能构成一组基";
    end try;
end proc:

(*
* 计算所有结果
* 输入：一组生成元
* 输出：
*     AD    伴随变换矩阵的数组
*     ADA    A[1]*A[2]*...*A[n]
*     dts    不变量数组
*)
getTransMatAndPDE:=proc(vv::list)
    local tbs,stbs,vvv,M,n,sbs,i,j,A,tmpv,MK,AD,ADA,ADT,BA,pPhi,eq,AList,dts,eqs;

    if not (indets(vv,name) subset SymsHolder:-syms) then
        error "表达式只能包含以下变量 %1, 可以使用 setSymbols 设置变量集合，但是不能包含 %2",
            SymsHolder:-syms,SymsHolder:-pnames;
    end if;

    vvv:=expand(vv):
    n:=numelems(vvv):
    sbs:=Vector[row](1..n,i->v[i]):# 生成元的表示符号
    flogf[2]("Input:");
    flog[2](seq(sbs[i]=vv[i],i=1..n));

    # 计算交换子矩阵，这里得到的是关于f的结果，需要进一步用基表示
    M:=Matrix(1..n, 1..n, (i, j)->vvv[i] &c vvv[j]):# 交换子的表达式形式
    MK:=Matrix(1..n,1..n);# 交换子关于生成元的系数向量矩阵

    # 将原交换子表用基表出
    tbs:=getKd~(vvv):# 非线性项及其系数映射表
    stbs:={map(indices,tbs,nolist)[]}:# 非线性项集
    # 生成元关于非线性项集的系数矩阵
    A:=Matrix(1..numelems(stbs),1..numelems(tbs),
    (i,j)->`if`(assigned(tbs[j][stbs[i]]),tbs[j][stbs[i]],0)):
    # 计算每个交换子关于生成元的系数
    # 这里将交换子的表达式转化成了生成元的线性表出
    for i from 1 to n do
        for j from 1 to n do
            if (M(i,j)<>0) then
                if (i<=j) then
                    tmpv:=ans2v(M(i,j),A,stbs);
                    M(i,j):=sbs.tmpv;
                    MK(i,j):=convert(tmpv,list);
                else
                    M(i,j):=-M(j,i);
                    MK(i,j):=-MK(j,i);
                end if;
            else
                MK(i,j):=convert(Vector[row](1..n),list);
            end if;
        end do;
    end do;
    flogf[2]("Commutator table:");
    flog[2](M);

    # 伴随矩阵
    AD:=Array(1..n);
    ADA:=LinearAlgebra[IdentityMatrix](n);
    ADT:=Matrix(1..n,1..n);
    flogf[2]("Adjoint transformation matrixes :");
    for i from 1 to n do
        AD[i]:=LinearAlgebra[MatrixExponential](Matrix(convert(MK[i],list)),-epsilon[i]);
        ADA:=ADA.AD[i];
        ADT(i,1..n):=subs~(epsilon[i]=epsilon,(AD[i].sbs^%T)^%T);
        flogf[2]("Adjoint transformation matrix of %a",sbs[i]);
        flog[2](AD[i]);
    end do;
    flogf[2]("General adjoint transformation matrix");
    flog[2](ADA);

    flogf[2]("Adjoint representation table:");
    flog[2](ADT);

    # 计算不变量
    BA:=Matrix(1..n,1..n,(i,j)->b[i]*a[j]);
    pPhi:=add(BA*~M);
    eqs:=getPDE(pPhi,sbs);
    return AD,ADA,eqs;
end proc:

# 计算不变量
# 已化简并排序
getInvariants:=proc(eqs)
    local res;
    res:=pdsolve(eqs);
    res:=res[];
    res:=[op(op(2,res))];
    res:=sortByComplexity(res);# 按照复杂度升序输出
    flogf[0]("解得的不变量");
    map(x->flog[0]('Delta'[x]=res[x]),[seq(i,i=1..numelems(res))]);
    res:=simplifyInvariants(res);# 不变量化简
    res:=sortByComplexity(res);# 按照复杂度升序输出
    flogf[0]("化简后的不变量");
    map(x->flog[0]('Delta'[x]=res[x]),[seq(i,i=1..numelems(res))]);
    return res;
end proc;

(*
* 生成不变量的偏微分方程组
* 输入：
*     p        p=[w,v],w=sum(b[j]*v[j]),v=sum(a[i]*v[i])
*     sbs      生成元符号集 v[1],...,v[n]
* 输出：
*     偏微分方程组
*)
getPDE:=proc(p,sbs)
    local n:=numelems(sbs),i,eq,eqs;
    uses phi=phi(seq(a[i],i=1..n));
    eq:=add(coeff(p,sbs[i])*diff(phi,a[i]),i=1..n);
    eqs:={seq(coeff(eq,b[i]),i=1..n)} minus {0};
end proc:

$endif


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

`eta/ord_simplex_interior` := proc(A::set)
 if nops(A) <> 1 then return FAIL; fi;

 return [`eta/ord`(A),`eta/simplex_interior`(A)];
end;

`gamma/ord_simplex_interior` := (A::set,B::set) -> (p) -> proc(U,V)
 local RU,RV,xU,xV,b;

 RU := U[1];
 RV := table([seq(b = eval(V[b][1]),b in B)]);
 xU := U[2];
 xV := table([seq(b = eval(V[b][2]),b in B)]);
 return [`gamma/ord`(A,B)(p)(RU,RV),
         `gamma/simplex_interior`(A,B)(p)(xU,xV)];
end;


######################################################################
# Operad of orderings

`eta/ord` := (A::set) -> `if`(nops(A)=1,[op(A)],FAIL);

`gamma/ord` := (A::set,B::set) -> (p) -> proc(R,RR)
 local b;
 if not check_gamma_args(A,B,p,R,RR,`is_element/ord`) then
  return FAIL;
 fi;

 map(op,[seq(RR[b],b in R)]);
end;


`is_element/autorel` := (A::set) -> proc(R)
 `is_element/rel`(A,A)(R);
end;

`is_equal/autorel` := (A::set) -> (R,S) -> `is_equal/rel`(A,A)(R,S);
`is_leq/autorel`   := (A::set) -> (R,S) -> `is_leq/rel`(A,A)(R,S);
`bot/autorel` := (A::set) -> `bot/rel`(A,A);
`top/autorel` := (A::set) -> `top/rel`(A,A);

`is_a_function/autorel` := (A::set) -> (R) -> `is_a_function/rel`(A,A)(R);
`is_total/autorel` := (A::set) -> (R) -> `is_total/rel`(A,A)(R);

`op/autorel` := (A::set) -> (R) -> `op/rel`(A,A)(R);

`hash/autorel` := (A::set) -> (R) -> `hash/rel`(A,A)(R);
`unhash/autorel` := (A::set) -> (r) -> `unhash/rel`(A,A)(r);

`id/autorel` := (A::set) -> `id/rel`(A);

`o/autorel` := (A::set) -> (S,R) -> `o/rel`(A,A,A)(S,R);

`is_reflexive/autorel` := (A::set) -> proc(R)
 return evalb(`id/autorel`(A) minus R = {});
end;

`is_irreflexive/autorel` := (A::set) -> proc(R)
 return evalb(`id/autorel`(A) intersect R = {});
end;

`is_symmetric/autorel` := (A::set) -> proc(R)
 return evalb(R = `op/autorel`(A)(R));
end;

`is_antisymmetric/autorel` := (A::set) -> proc(R)
 return evalb(R intersect `op/autorel`(A)(R) = {});
end;

`is_transitive/autorel` := (A::set) -> proc(R)
 local RR;
 RR := `o/autorel`(A)(R,R);
 return evalb(`is_leq/autorel`(A)(RR,R));
end;

`is_separated/autorel` := (A::set) -> proc(R)
 return evalb((R intersect `op/autorel`(A)(R)) minus `id/autorel`(A) = {});
end;

`list_elements/autorel` := (A::set) -> `list_elements/rel`(A,A);
`count_elements/autorel` := (A::set) -> 2^(nops(A)^2);

`random_element/autorel` := (A::set) -> (p_) -> `random_element/rel`(A,A)(args);

`transitive_closure/autorel` := (A::set) -> proc(R)
 local S,n0,n1;

 S := `id/autorel`(A) union R;
 n0 := 0;
 n1 := nops(S);
 while n1 > n0 do
  n0 := n1;
  S := `o/autorel`(A)(S,S);
  n1 := nops(S);
 od;

 return S;
end;

`cofunctor/autorel` := (A::set,B::set) -> (f) -> proc(R)
 local AA;

 AA := `top/autorel`(A);
 return select(aa -> member([f[aa[1]],f[aa[2]]],R),AA);
end;

`reindex_standard/autorel` := (A::set) -> proc(R)
 local n,i,f,N,R0;

 n := nops(A);
 f := table();
 for i from 1 to n do f[A[i]] := i; od;
 N := {seq(i,i=1..n)};
 R0 := map(aa -> [f[aa[1]],f[aa[2]]],R);
 return [N,R0];
end;


single_case_Partition := module()
  uses Domain;

  export SimplName  := "Single case partition";
  export SimplOrder := 11;

    export ModuleApply := proc(vs :: DomBound, sh :: DomShape, $)
        subsindets(sh, DomSplit, x->do_simp(op(x)));
    end proc;

    local do_simp := proc(p:: Partition,$)::DomShape;
        local r := Partition:-Simpl:-single_nonzero_piece_cps(
            proc(c,v) if v::DomConstrain then DConstrain(conv_bool(c),op(v)) else p end if
            end proc,p,_testzero=(x->x=DSum()));
        if r :: Partition then DSplit(r) else r end if;
    end proc;

    local conv_bool := proc(r, $)
        if r :: {specfunc(`And`), `and`} then
            op(map(conv_bool,r))
        else
            r
        end if;
    end proc;
end module;

redundant_Partition_Pieces := module()
  uses Domain;

  export SimplName  := "Redundant Partition pieces";
  export SimplOrder := (10+1/2);

  export ModuleApply := proc(vs :: DomBound, sh :: DomShape, $)
    local as := Domain:-Bound:-toConstraints(vs, 'bound_types');
    subsindets(sh, DomSplit, proc(pr)
        local r; r := Partition:-Simpl(op(1,pr)) assuming op(as);
        if not r :: Partition then r else pr end if;
      end proc);
  end proc;
end module;