# NewtonEuler Formulation for Robot based on MDH frames

# Einleitung
# Berechnung der inversen Dynamik in Rückwärtsrekursive
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# tree -> Berechnung für eine beliebige Baumstruktur (ohne Schleifen)
# fixb ->  Fixed Base Modell der Basis.
# dynamics -> Berechnung der Dynamik
# NewtonEuler->Newton - Euler Bewegungsgleichung
# Linkframe->Berechnung der Newton Euler im Körper-KS(KSi)
# par12 -> Parametersatz 1 (Schwerpunkt als Parameter: SX,SY,SZ) oder Parametersatz 2 (1. und 2. Moment MX,MY,MZ,...)

# Sources
# [GautierKhalil1990] Direct Calculation of Minimum Set of Inertial Parameters of Serial Robots
# [KhalilDombre2002] Modeling, Identification and Control of Robots
# [Ortmaier2014] Vorlesungsskript Robotik I
#
# 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):
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotx":
read "../transformation/proc_troty":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../transformation/proc_trafo_mdh":
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
codegen_opt := 2:
codeexport_grav := true:
codeexport_corvec := true:
codeexport_cormat := true:
codeexport_inertia := true:
codeexport_inertiaD := true:
codeexport_invdyn := true:
codegen_act := true:
codegen_dynpar := 2:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../robot_codegen_definitions/robot_env":
printf("Generiere Dynamikgleichungen für %s (Herleitung im Körper-KS)\n", robot_name):
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
read sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
kin_constraints_exist := kin_constraints_exist: # nur zum Abschätzen der Komplexität
;
# Term-Vereinfachungen einstellen
if not assigned(simplify_options) or simplify_options(9)=-1 then # Standard-Einstellungen:
  if not kin_constraints_exist then # normale serielle Ketten und Baumstrukturen
    use_simplify := 0: # Standardmäßig aus
  else # mit kinematischen Zwangsbedingungen
    if NJ <= 5 then # nur bei einfachen Systemen Term-Vereinfachung durchführen
      use_simplify := 1:
    else
      use_simplify := 0: # Annahme: Dauert zu lange bzw. zu rechenintensiv (nicht geprüft)
    end if:
  end if:
else # Benutzer-Einstellungen:
  use_simplify := simplify_options(9): # neunter Eintrag ist für Dynamik
end if:

read "../helper/proc_index_symmat2vector":
read "../helper/proc_symmat2vector":
# Ergebnisse der Kinematik laden
read sprintf("../codeexport/%s/tmp/kinematics_floatb_%s_rotmat_maple.m", robot_name, base_method_name):
Trf := Trf:
Trf_c := Trf_c:
# Ergebnisse der Geschwindigkeit laden
read sprintf("../codeexport/%s/tmp/velocity_linkframe_floatb_%s_maple.m", robot_name, base_method_name):
omega_i_i:= omega_i_i:
rD_i_i:= rD_i_i:
# Zeitableitungen der MDH-Drehwinkel laden.
# Die Berechnung soll nur an einer Stelle erfolgen. Siehe robot_tree_velocity_mdh_angles.mw.
read sprintf("../codeexport/%s/tmp/velocity_mdh_angles_maple.m", robot_name):
thetaD := thetaD:
dD :=dD:
# Zeitableitungen der Geschwindigkeit laden.
# Die Berechnung soll nur an einer Stelle erfolgen. Siehe robot_tree_acceleration_mdh_angles.mw.
read sprintf("../codeexport/%s/tmp/acceleration_mdh_angles_maple.m", robot_name):
thetaDD :=thetaDD:
dDD := dDD:
# Ergebnisse der Geschwindigkeit laden. TODO: Warum doppelt?
read sprintf("../codeexport/%s/tmp/velocity_linkframe_floatb_%s_maple.m", robot_name, base_method_name):
omega_i_i:= omega_i_i:
rD_i_i:= rD_i_i:
# Ergebnisse der Beschleunigung laden
accfile := sprintf("../codeexport/%s/tmp/acceleration_linkframe_floatb_%s_maple.m", robot_name, base_method_name):
if FileTools[Exists](accfile) then
  read accfile:
else
  printf("%s. Schwerpunktsbeschleunigung wurde nicht berechnet. Abbruch der Newton-Euler-Berechnung.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  quit: # Funktioniert in GUI nicht richtig...
  robot_name := "": # ...Daher auch Löschung des Roboternamens.
end if:
omegaD_i_i:= omegaD_i_i:
rDD_i_i:= rDD_i_i:
rDD_i_Si:= rDD_i_Si:
# Mit diesem Arbeitsblatt werden die Vorwärtsrekursive für Fixed-Base Modelle generiert. Erkenne welche Basis-Modellierung aktiv ist
if base_method_name="twist" then # Basis-Methode "twist" wird (hier) nur für fixed Base benutzt
  expstring:="fixb":
elif base_method_name="eulxyz" then
  expstring:="floatb_eulxyz":
else
  printf("Nicht behandelte Basis-Methode: %s\n", base_method_name):
end if:
# Schalter zur Auswahl der unterschiedlichen Terme, die exportiert werden sollen.

# Newton-Euler formulation(mit Funktion)
OutputNewtonEuler:= NewtonEuler(f_i_i,m_i_i):
# Backward recursive computation
# Force exerted on link i by link i-1(unbekannte Kraft zwischen Teilkörper n-1 und Teilkörper n)
f_i_i:= Matrix(3,NJ+1):
# Moment exerted on link i by link i-1(unbekannte Moment zwischen Teilkörper n-1 und Teilkörper n)
m_i_i:=Matrix(3,NJ+1):

# Schnittmomente/Kräfte
tau_B := Matrix(6,1):
tau_J := Matrix(NQJ,1):
tau := Matrix(6+NQJ,1):

I_i_i_m:= Matrix(3,3,NL):
for i from 1 to NL do
  I_i_i_m(1..3,1..3,i):=Matrix(3,3):
 end do:

for i from 1 to NL do
  I_i_i_m[1,1,i]:=I_i_i[1,i]:
  I_i_i_m[1,2,i]:=I_i_i[2,i]:
  I_i_i_m[1,3,i]:=I_i_i[3,i]:
  I_i_i_m[2,1,i]:=I_i_i[2,i]:
  I_i_i_m[2,2,i]:=I_i_i[4,i]:
  I_i_i_m[2,3,i]:=I_i_i[5,i]:
  I_i_i_m[3,1,i]:=I_i_i[3,i]:
  I_i_i_m[3,2,i]:=I_i_i[5,i]:
  I_i_i_m[3,3,i]:=I_i_i[6,i]:
end do:

F_i:= Matrix(3,NL):
M_i:= Matrix(3,NL):
for i from 1 to NL do
  # External forces and moment on link i
  #Betrachte dazu nur die durch Gelenke angetriebenen Körper, nicht die Basis
  # [Khali2002](9.5.3) (9.88) & (9.89)
  F_i(1..3,i):= M(i,1)*rDD_i_i(1..3,i)+CrossProduct(omegaD_i_i(1..3,i),mr_i_i_Si(1..3,i))+ CrossProduct(omega_i_i(1..3,i), CrossProduct(omega_i_i(1..3,i), mr_i_i_Si(1..3,i))):
  M_i(1..3,i):= I_i_i_m(1..3,1..3,i).omegaD_i_i(1..3,i)+ CrossProduct(omega_i_i(1..3,i),(I_i_i_m(1..3,1..3,i).omega_i_i(1..3,i))) + CrossProduct(mr_i_i_Si(1..3,i),rDD_i_i(1..3,i)):#CrossProduct(r,F_i(1..3,i)):####
  # Terme vereinfachen
  if use_simplify>=1 then
    tmp_t1:=time():
    tmp_l11 := length(F_i(1..3,i)):
    tmp_l12 := length(M_i(1..3,i)):
    F_i(1..3,i) := simplify2(F_i(1..3,i)):
    M_i(1..3,i) := simplify2(M_i(1..3,i)):
    tmp_l21 := length(F_i(1..3,i)):
    tmp_l22 := length(M_i(1..3,i)):
    tmp_t2:=time():
    printf("%s: Terme für inneres Kraft/Moment %d vereinfacht. Länge: %d->%d / %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i, tmp_l11, tmp_l21, tmp_l12, tmp_l22, tmp_t2-tmp_t1):
  end if:
end do :

# Forces and moment exerted on link i by link i-1

for i from NL by -1 to 1 do
  f_i_i_part:= F_i(1..3,i):
  m_i_i_part:= M_i(1..3,i):# + CrossProduct(r_i_i_Si(1..3,i),(F_i(1..3,i))):

  I_nf := Matrix([ListTools[SearchAll](i-1,convert(v,list))]):

  for tmp from 1 to ColumnDimension(I_nf) do
    I_nf(tmp) := I_nf(tmp)+1:
  end do:

  if ColumnDimension(I_nf) <> 0 then
    for tmp from 1 to RowDimension(v) do
      j := I_nf(tmp):

      R_i_j := Matrix(Trf(1..3,1..3,j-1)):
      r_i_i_j := Matrix(Trf(1 .. 3, 4, j-1)):
      f_j_j := f_i_i(1..3,j):
      m_j_j := m_i_i(1..3,j):

      f_i_i_part := f_i_i_part + R_i_j.f_j_j:
      m_i_i_part := m_i_i_part + R_i_j.m_j_j + CrossProduct(r_i_i_j, R_i_j.f_j_j):

      if tmp = ColumnDimension(I_nf) then
        break: #Abbruch. Alle Nachfolger untersucht.
      end if:
    end do:
  end if:

  f_i_i(1..3,i) := f_i_i_part;
  m_i_i(1..3,i) := m_i_i_part;
  # Terme vereinfachen
  if use_simplify>=1 then
    tmp_t1:=time():
    tmp_l11 := length(f_i_i(1..3,i)):
    tmp_l12 := length(m_i_i(1..3,i)):
    f_i_i(1..3,i) := simplify2(f_i_i(1..3,i)):
    m_i_i(1..3,i) := simplify2(m_i_i(1..3,i)):
    tmp_l21 := length(f_i_i(1..3,i)):
    tmp_l22 := length(m_i_i(1..3,i)):
    tmp_t2:=time():
    printf("%s: Rekursive Terme für Kraft/Moment %d vereinfacht. Länge: %d->%d / %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l11, tmp_l21, tmp_l12, tmp_l22, tmp_t2-tmp_t1):
  end if:
end do:

for i from 2 to NL do # Schleife über Anzahl der Körper
  if sigma(i-1) <> 2 then
    if sigma(i-1) = 0 then # Drehgelenk
      tau_J(i-1,1) := <0,0,1>.m_i_i(1..3,i):
    else # Schubgelenk
      tau_J(i-1,1) := <0,0,1>.f_i_i(1..3,i):
    end if:
  end if:
end do:
# Unbekannten Kräfte und Momente zwischen den Teilkörpern bestimmen
tau_B [1..3,1]:= Transpose(Trf_c(1..3, 1..3, 1)).f_i_i(1..3,1) :
tau_B [4..6,1]:= Transpose(T_basevel).Trf_c(1..3, 1..3, 1).m_i_i(1..3,1):

tau[1..6,1]:= tau_B:
tau[7..6+NQJ,1]:= tau_J:
# Export
f_i_i := convert_t_s(f_i_i):
m_i_i := convert_t_s(m_i_i):
tau_J := convert_t_s(tau_J):
tau_B := convert_t_s(tau_B):
tau := convert_t_s(tau):
# Floating Base
f_i_i_floatb := copy(f_i_i):
m_i_i_floatb := copy(m_i_i):
tau_J_floatb := copy(tau_J):
tau_B_floatb := copy(tau_B):
tau_floatb := copy(tau):
for i from 1 to 6 do
  if i < 4 then
    var := VD_base_s[i,1] - g_world[i,1];
  else
    var := VD_base_s[i,1]:
  end if:
  f_i_i_floatb := subs({VD_base_s[i,1]=var},f_i_i_floatb):
  m_i_i_floatb := subs({VD_base_s[i,1]=var},m_i_i_floatb):
  tau_J_floatb := subs({VD_base_s[i,1]=var},tau_J_floatb):
  tau_B_floatb := subs({VD_base_s[i,1]=var},tau_B_floatb):
  tau_floatb := subs({VD_base_s[i,1]=var},tau_floatb):
end do:
# Maple Export # TODO: Wieso Bedingung "twist" gewählt?
if not(base_method_name="twist") then
  save  f_i_i_floatb,m_i_i_floatb,tau_J_floatb, tau_B_floatb, tau_floatb,sprintf("../codeexport/%s/tmp/invdyn_%s_NewtonEuler_linkframe_maple.m", robot_name, base_method_name):
  printf("%s. Maple-Ausdrücke exportiert.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
end if:
# Matlab Export
if codegen_act and codeexport_invdyn and not(base_method_name="twist") then
  MatlabExport(convert_t_s(f_i_i_floatb), sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_NewtonEuler_linkframe_f_i_i_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
  MatlabExport(convert_t_s(m_i_i_floatb), sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_NewtonEuler_linkframe_m_i_i_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
  MatlabExport(convert_t_s(tau_J_floatb), sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_NewtonEuler_linkframe_tauJ_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
  MatlabExport(convert_t_s(tau_B_floatb), sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_NewtonEuler_linkframe_tauB_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
  MatlabExport(convert_t_s(tau_floatb), sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_NewtonEuler_linkframe_tauJB_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
# Fixed Base
for i from 1 to NQB do
  f_i_i := subs({X_base_s[i,1]=0},f_i_i):
  m_i_i := subs({X_base_s[i,1]=0},m_i_i):
  tau_J := subs({X_base_s[i,1]=0},tau_J):
  tau_B := subs({X_base_s[i,1]=0},tau_B):
  tau := subs({X_base_s[i,1]=0},tau):
end do:
for i from 1 to 6 do
  f_i_i := subs({V_base_s[i,1]=0},f_i_i):
  m_i_i := subs({V_base_s[i,1]=0},m_i_i):
  tau_J := subs({V_base_s[i,1]=0},tau_J):
  tau_B := subs({V_base_s[i,1]=0},tau_B):
  tau := subs({V_base_s[i,1]=0},tau):
  if i < 4 then
    var := -g_world[i,1];
  else
    var := 0:
  end if:
  f_i_i := subs({VD_base_s[i,1]=var},f_i_i):
  m_i_i := subs({VD_base_s[i,1]=var},m_i_i):
  tau_J := subs({VD_base_s[i,1]=var},tau_J):
  tau_B := subs({VD_base_s[i,1]=var},tau_B):
  tau := subs({VD_base_s[i,1]=var},tau):
end do:
# Maple Export
save  f_i_i,m_i_i,tau_J, tau_B, tau,sprintf("../codeexport/%s/tmp/invdyn_%s_NewtonEuler_linkframe_par%d_maple.m", robot_name, base_method_name, codegen_dynpar):
printf("%s. Maple-Ausdrücke exportiert.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
# Matlab Export
if codegen_act and codeexport_invdyn  then
  MatlabExport(f_i_i, sprintf("../codeexport/%s/tmp/invdyn_fixb_NewtonEuler_linkframe_f_i_i_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt):
  MatlabExport(m_i_i, sprintf("../codeexport/%s/tmp/invdyn_fixb_NewtonEuler_linkframe_m_i_i_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt):
  MatlabExport(tau_J, sprintf("../codeexport/%s/tmp/invdyn_fixb_NewtonEuler_linkframe_tauJ_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt):
  MatlabExport(tau_B, sprintf("../codeexport/%s/tmp/invdyn_fixb_NewtonEuler_linkframe_tauB_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt):
  MatlabExport(tau, sprintf("../codeexport/%s/tmp/invdyn_fixb_NewtonEuler_linkframe_tauJB_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt):
end if:


(*
 Copyright (C) 2019 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_x_ncap_params *params;

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

ncap_f0 := s -> 1 + params_a_mu*tanh(s)*arcsinh(s)*( 1 + params_a_alpha*((1-params_a_zeta)*s*log(1+s) + params_a_zeta*s))/(1 + params_a_beta*tanh(s)*arcsinh(s)):
ncap_f  := x -> ncap_f0(X2S*x):

f := (rs, zeta, xt, xs0, xs1) -> gga_exchange(ncap_f, rs, zeta, xs0, xs1):


(*
 Copyright (C) 2019 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: mgga_exc *)
(* prefix:
  mgga_x_task_params *params;

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

task_alpha := (x, t) -> (t/K_FACTOR_C) * m_max(1 - x^2/(8*t), 1e-10):

task_gx0 := x -> 1 - exp(-params_a_task_c*x^(-1/4)):
task_gx := x -> my_piecewise3(x > 0, task_gx0(m_max(x, 0)), 0):

task_hx1 := r -> simplify(add(params_a_task_anu[i+1]*ChebyshevT(i, (r - 1)/(r + 1)), i=0..2)):

task_fx  := r -> simplify(add(params_a_task_bnu[i+1]*ChebyshevT(i, (r - 1)/(r + 1)), i=0..4)):

task_f0 := (s, a) -> params_a_task_h0x*task_gx(s^2) +
  (1.0 - task_fx(a))*(task_hx1(s^2) - params_a_task_h0x)*task_gx(s^2)^params_a_task_d:

task_f := (x, u, t) -> task_f0(X2S*x, task_alpha(x, t)):

f := (rs, z, xt, xs0, xs1, u0, u1, t0, t1) ->
  mgga_exchange(task_f, rs, z, xs0, xs1, u0, u1, t0, t1):


with(DifferentialAlgebra):
ring_diff := DifferentialRing(blocks = [[x3, x1, x2, x0], [y1] ], derivations = [t]):
sys := [
diff(x3(t), t) - (-b2*x3(t) + b2*x2(t)),
diff(x1(t), t) - (a2*x0(t) - a2*x1(t)),
diff(x2(t), t) - ((ka*kc*b1*x3(t) - ka*kc*b1*x2(t) + ka*b1*x0(t)*x3(t) - ka*b1*x0(t)*x2(t) - n*kc*x2(t) + kc*b1*x3(t)*x2(t) - kc*b1*x2(t)^2) / (ka*kc + ka*x0(t) + kc*x2(t))),
diff(x0(t), t) - ((-ka*n*x0(t) - ka*kc*a1*x0(t) + ka*kc*a1*x1(t) - ka*a1*x0(t)^2 + ka*a1*x0(t)*x1(t) - kc*a1*x0(t)*x2(t) + kc*a1*x1(t)*x2(t)) / (ka*kc + ka*x0(t) + kc*x2(t))),
y1(t) - (x0(t))
];
res := CodeTools[CPUTime](RosenfeldGroebner(sys, ring_diff, singsol=none));


(*
 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: mgga_exc *)

$define lda_c_pw_params
$include "lda_c_pw.mpl"

$define lda_x_params
$include "lda_x.mpl"

cc06_cnst  := (3/(4*Pi))^(2/3):

cc06_alpha := -0.0007:
cc06_beta  :=  0.0080*cc06_cnst:
cc06_gamma :=  0.026 *cc06_cnst:

cc06_f := (rs, z, us0, us1) ->
  (f_lda_x(rs, z) + f_pw(rs, z))*(1 +
    (cc06_alpha + cc06_beta*u_total(z, us0, us1))/(1 + cc06_gamma*u_total(z, us0, us1))
  ):

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


# A cactus lobe is a map f : R/Z -> R/Z of degree one that has
# constant speed on some finite collection of half-open intervals,
# and speed zero on the complement of those intervals.  We encode
# f as a pair [c,T], where c = f(0) is in R/Z (as defined by
# `is_element/RZ`) and T is the half-open interval where the
# speed is nonzero (as defined by `is_element/RZ_set`).

`is_element/cactus_lobes` := proc(cT)
 global reason;
 local c,T;

 if not(type(cT,list) and nops(cT) = 2) then
  reason := ["is_element/cactus_lobes","cT is not a list of length 2",cT];
  return false;
 fi;

 c,T := op(cT);

 if not `is_element/RZ`(c) then
  reason := ["is_element/cactus_lobes","c is not in R/Z",c,cT];
  return false;
 fi;

 if not `is_element/RZ_set`(T) then
  reason := ["is_element/cactus_lobes","T is not an RZ_set",T,cT];
  return false;
 fi;

 if T = `empty/RZ` then
  reason := ["is_element/cactus_lobes","T is empty",T,cT];
  return false;
 fi;

 return true;
end:

`is_equal/cactus_lobes` := proc(cT0,cT1)
 return `is_equal/RZ`(cT0[1],cT1[1]) and
        `is_equal/RZ_set`(cT0[2],cT1[2]);
end:

`random_element/cactus_lobes` := proc()
 local T;
 T := `empty/RZ_set`;
 while T = `empty/RZ_set` do T := `random_element/RZ_set`(); od:
 return [`random_element/RZ`(),T];
end:

`is_leq/cactus_lobes` := NULL:
`list_elements/cactus_lobes` := NULL:
`count_elements/cactus_lobes` := NULL:

`start/cactus_lobes` := (cT) -> cT[1];
`motion_set/cactus_lobes` := (cT) -> cT[2];
`speed/cactus_lobes` := (cT) -> 1/`length/RZ_set`(cT[2]);

`eval/cactus_lobes` := (cT) -> proc(t)
 local t0,c,T,n,i,u,s;

 c,T := op(cT);
 n := nops(T)/2;
 i := 1;
 u := c;
 s := 1/`length/RZ_set`(T);
 t0 := t - floor(t);
 while i <= n and T[2*i-1] <= t0 do
  u := u + s*(min(t0,T[2*i]) - T[2*i-1]);
  i := i+1;
 od:

 u := u - floor(u);
 return u;
end:

`id/cactus_lobes` := [0,[0,1]];

`o/cactus_lobes` := proc(cT,dU)
 local c,T,d,U,n,m,sT,sU,T1,T2,n1,V,a,b,y,z,i,j,aa,bb;
 c,T := op(cT);
 d,U := op(dU);
 n := nops(T)/2;
 m := nops(U)/2;
 sT := 1/`length/RZ_set`(T);
 sU := 1/`length/RZ_set`(U);
 T1 := `unroll/RZ_set`(T);
 n1 := nops(T1)/2;
 V := NULL;
 y := d;
 for i from 1 to m do
  a := U[2*i-1];
  b := U[2*i];
  z := y + sU * (b - a);
  T2 := (T1 -~ y)/~sU +~ a;
  for j from 1 to n1 do
   if T2[2*j] >= a and T2[2*j-1] <= b then
    aa := max(T2[2*j-1],a);
    bb := min(T2[2*j],b);
    if aa < bb then
     V := V,aa,bb;
    fi;
   fi;
  od;
  y := z;
 od;

 return [`eval/cactus_lobes`(cT)(d),[V]];
end:

`source_shift/cactus_lobes` := (t) -> proc(cT)
 local c,d,T,U;
 c,T := op(cT);
 U := `shift/RZ_set`(t)(T);
 d := `eval/cactus_lobes`(cT)(-t);
 return [d,U];
end:

`target_shift/cactus_lobes` := (t) -> proc(cT)
 local c,d,T;
 c,T := op(cT);
 d := c+t;
 d := d - floor(d);
 return [d,T];
end:

`plot/cactus_lobes` := proc(cT)
 local c,T,L,x,y,z,a,b,n,i,s,A;
 c,T := op(cT);
 L := NULL;
 if T[1] > 0 then
  L := L,[[0,c],[T[1],c]];
 fi;
 y := c;
 n := nops(T)/2;
 s := 1/`length/RZ_set`(T);
 for i from 1 to n do
  a := T[2*i-1];
  b := T[2*i];
  z := y + s * (b-a);
  if z <= 1 then
   L := L,[[a,y],[b,z]];
   y := z;
  else
   x := a + (1 - y)/s;
   L := L,[[a,y],[x,1]],[[x,0],[b,z-1]];
   y := z-1;
  fi;
  x := `if`(i < n,T[2*i+1],1);
  if b < x then
   L := L,[[b,y],[x,y]];
  fi;
 od;

 A := args[2..-1];
 return
  display(map(u -> line(u[1],u[2],A),[L]),scaling=constrained,axes=none);
end:

`full_plot/cactus_lobes` := proc(cT)
 local x;

 display(
  `plot/cactus_lobes`(cT),
  seq(line([x,0],[x,1],colour=red),x in [0,1]),
  seq(line([x,0],[x,1],colour=red,linestyle=dot),x in cT[2]),
  line([0,0],[1,0],colour=blue),
  line([0,1],[1,1],colour=blue),
  line([0,cT[1]],[1,cT[1]],colour=green,linestyle=dash)
 );
end:


(*
 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: mgga_exc *)

$define lda_c_pw_params
$include "lda_c_pw.mpl"

$define lda_x_params
$include "lda_x.mpl"

b98_gamma_x  := 0.11:
b98_gamma_ss := 1.6:
b98_gamma_ab := 0.14:

params_a_c_x  := [0.8085,  0.6682, 0.1420]:
params_a_c_ss := [0.2606, -0.9608, 0.9023]:
params_a_c_ab := [1.2033, -2.2717, 0.9596]:

b98_q := (x, u, t) -> 1 - (t - x^2/8 - u/4)/K_FACTOR_C:

b98_g := (gamma, cc, q) -> add(cc[i]*(gamma*q/sqrt(1 + gamma^2*q^2))^(i-1), i=1..3):

b98_f := (rs, z, xs0, xs1, us0, us1, ts0, ts1) ->
  + lda_x_spin(rs,  z)
    * b98_g( b98_gamma_x, params_a_c_x, b98_q(xs0, us0, ts0))
  + lda_x_spin(rs, -z)
    * b98_g( b98_gamma_x, params_a_c_x, b98_q(xs1, us1, ts1))
  + lda_stoll_par(f_pw, rs,  z,  1)
    * b98_g(b98_gamma_ss, params_a_c_ss, b98_q(xs0, us0, ts0)) * Fermi_D(xs0, ts0)
  + lda_stoll_par(f_pw, rs, -z, -1)
    * b98_g(b98_gamma_ss, params_a_c_ss, b98_q(xs1, us1, ts1)) * Fermi_D(xs1, ts1)
  + lda_stoll_perp(f_pw, rs, z)
    * b98_g(b98_gamma_ab, params_a_c_ab, (b98_q(xs0, us0, ts0) + b98_q(xs1, us1, ts1))/2):

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


# A full flag of subspaces in (Z/p)^d is represented by a list
# [w[1],...,w[d]] where the subspace of dimension i is
# spanned by [w[1],...,w[i]] and the last nonzero entry in
# w[i] is a 1 in position h[i] say, and w[i+1],...,w[d]
# all have zero in position h[i].

`count_elements/vectors` := (p,d) -> p^d;

`list_elements/vectors` := proc(p,d)
 local L,i,j,u;

 L := [[]];
 for i from 1 to d do
  L := [seq(seq([op(u),j],j=0..p-1),u in L)];
 od:
 return L;
end:

`height/vectors` := (p,d) -> proc(u)
 local h;

 h := d;
 while h > 0 and u[h] = 0 do h := h - 1; od;
 return h;
end:

`count_elements/monic_vectors` := (p,d) -> (p^d - 1)/(p - 1);

`list_elements/monic_vectors` := proc(p,d)
 local i,u;

 [seq(seq([op(u),1,0$(d-1-i)],u in `list_elements/vectors`(p,i)),i=0..d-1)];
end:

`count_elements/full_flags` := proc(p,d) local i; mul((p^i-1)/(p-1),i=1..d); end:

`list_elements/full_flags` := proc(p,d)
 local L,L1,nL,H,V,i,j,k,u,v,q;
 L := [[]];
 H := [[]];

 for i from 1 to d do
  nL := nops(L);
  L1 := L;
  L := [];
  V := `list_elements/monic_vectors`(p,d - i + 1);
  H := map(W -> sort(map(`height/vectors`(p,d),W)),L1);
  for j from 1 to nL do
   for v in V do
    u := [];
    q := 1;
    for k from 1 to d do
     if member(k,H[j]) then
      u := [op(u),0];
     else
      u := [op(u),v[q]];
      q := q + 1;
     fi;
    od:
    L := [op(L),[op(L1[j]),u]];
   od;
  od:
 od:

 return L;
end:

`random_element/full_flags` := (p,d) -> proc()
 local V,i,v;
 V := table():
 V[0] := `bot/subspaces`(p,d);
 for i from 1 to d do
  v := [0$d];
  while `is_member/subspaces`(p,d)(v,V[i-1]) do
   v := [seq(rand(0..p-1)(),i=1..d)];
  od;
  V[i] := `span/subspaces`(p,d)([op(V[i-1][1]),v]);
 end:
 return `from_subspace_list/full_flags`(p,d)([seq(V[i],i=1..d)]);
end:

`is_element/full_flags` := (p,d) -> proc(W)
 local h,i,j;

 if not(type(W,list(list(integer)))) then
  return false;
 fi;

 if map(nops,W) <> [d$d] then
  return false;
 fi;

 if W <> modp(W,p) then
  return false;
 fi;

 h := table():
 for i from 1 to d do
  if W[i] = [0$d] then return false; fi;
  j := d;
  while W[i][j] = 0 do j := j - 1; od;
  if W[i][j] <> 1 then
   return false;
  fi;
  h[i] := j;
 od:

 for i from 1 to d do
  for j from i+1 to d do
   if W[j][h[i]] <> 0 then return false; fi;
  od;
 od;

 return true;
end:

`count_elements/full_flags` := proc(p,d) local i; mul((p^i-1)/(p-1),i = 1..d); end:

`basepoint/full_flags` := (p,d) -> convert(IdentityMatrix(d),listlist);

`to_subspace_list/full_flags` := (p,d) -> proc(W)
 local i;
 [seq(`span/subspaces`(p,d)([op(1..i,W)]),i=1..d)];
end:

`from_subspace_list/full_flags` := (p,d) -> proc(V)
 local i,j,h,v,W;

 v := V[1][1][1];
 j := d; while j > 0 and v[j] = 0 do j := j - 1; od;
 v := modp(v /~ v[j],p);
 W := [v];
 h := [j];
 for i from 2 to d do
  j := 1;
  while j < i and `is_member/subspaces`(p,d)(V[i][1][j],V[i-1]) do
   j := j + 1;
  od;
  v := V[i][1][j];
  for j from 1 to i-1 do
   v := modp(v -~ v[h[j]] *~ W[j],p);
  od:
  j := d; while j > 0 and v[j] = 0 do j := j - 1; od;
  v := modp(v /~ v[j],p);
  W := [op(W),v];
  h := [op(h),j];
 od:
 return W;
end:

# It would be better to do this directly and use it in the
# `from_subspace/full_flags` procedure.
`from_generators/full_flags` := (p,d) -> proc(U)
 local V,i;
 V := [seq(`span/subspaces`(p,d)([op(1..i,U)]),i=1..d)];
 return `from_subspace_list/full_flags`(p,d)(V);
end:

`matrix_act/full_flags` := (p,d) -> proc(g,W)
 local gW,V,i;

 gW := convert(Transpose(Matrix(g) . Transpose(Matrix(W))),listlist);
 V := [seq(`span/subspaces`(p,d)([op(1..i,gW)]),i=1..d)];
 return `from_subspace_list/full_flags`(p,d)(V);
end:

`jordan_permutation/full_flags` := (p,d) -> proc(U,V)
 local U0,V0,UV,UV1,i,j,w;
 U0 := `to_subspace_list/full_flags`(p,d)(U);
 U0 := table([0 = `bot/subspaces`(p,d),seq(i = U0[i],i=1..d)]);
 V0 := `to_subspace_list/full_flags`(p,d)(V);
 V0 := table([0 = `bot/subspaces`(p,d),seq(i = V0[i],i=1..d)]);
 UV := table():
 for i from 0 to d do
  for j from 0 to d do
   UV[i,j] := `inter/subspaces`(p,d)(U0[i],V0[j]);
  od;
 od;
 UV1 := table();
 w := table():
 for i from 1 to d do
  for j from 1 to d do
   UV1[i,j] := `sum/subspaces`(p,d)(UV[i-1,j],UV[i,j-1]);
   if `dim/subspaces`(p,d)(UV[i,j]) > `dim/subspaces`(p,d)(UV1[i,j]) then
    w[i] := j;
   fi;
  od;
 od;
 return [seq(w[i],i=1..d)];
end:


######################################################################
# F(N)(A) is Inj(A,RR^N) modulo translation and scaling.
#
# Elements are represented as tables indexed by A, whose values
# are lists of length N of real numbers.  The equivalence relation
# is built into the definition of the function `is_equal/F` rather
# than the representation of the elements.

`is_element/F` := (N::posint) -> (A::set) -> proc(x)
 local a;
 global reason;

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

 if map(op,{indices(x)}) <> A then
  reason := [convert(procname,string),"x is not indexed by A",x,A];
  return false;
 fi;

 for a in A do
  if not `is_element/R`(N)(x[a]) then
   reason := [convert(procname,string),"x[a] is not in R^N",a,x[a],N];
   return false;
  fi;
 od;

 if nops(map(op,{entries(x)})) <> nops(A) then
  reason := [convert(procname,string),"x is not injective",x];
  return false;
 fi;

 return true;
end;

######################################################################
# This function normalises the infinity norm.  This is convenient
# because it preserves rationality, but it is geometrically unpleasant.

`normalise/F` := (N::posint) -> (A::set) -> proc(x)
 local x0,u,r,a;

 x0 := table();
 u := [0$N];

 for a in A do
  x0[a] := x[a];
  u := u +~ x[a];
 od;
 u := u /~ nops(A);
 r := 0;
 for a in A do
  x0[a] := x0[a] -~ u;
  r := max(r,op(map(abs,x0[a])));
 od;
 for a in A do
  x0[a] := x0[a] /~ r;
 od;

 return(eval(x0));
end;

######################################################################
# This function normalises the euclidean norm
`normalise_2/F` := (N::posint) -> (A::set) -> proc(x)
 local x0,u,r2,r,a;

 x0 := table();
 u := [0$N];

 for a in A do
  x0[a] := x[a];
  u := u +~ x[a];
 od;
 u := u /~ nops(A);
 r2 := 0;
 for a in A do
  x0[a] := x0[a] -~ u;
  r2 := r2 + `norm_2/R`(N)(x0[a])^2;
 od;
 r := sqrt(r2);

 for a in A do
  x0[a] := x0[a] /~ r;
 od;

 return(eval(x0));
end;

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

`is_equal/F` := (N::posint) -> (A::set) -> proc(x,y)
 local x0,y0,a;
 global reason;

 x0 := `normalise/F`(N)(A)(x);
 y0 := `normalise/F`(N)(A)(y);

 for a in A do
  if not(`is_equal/R`(N)(x0[a],y0[a])) then
   reason := [convert(procname,string),"x0[a] <> y0[a]",a,x0[a],y0[a]];
   return false;
  fi;
 od;

 return true;
end;

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

`is_leq/F` := NULL;

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

`random_element/F` := (N::posint) -> (A::set) -> proc()
 local ok,x,a;

 ok := false;
 while not(ok) do
  x := table();
  for a in A do
   x[a] := `random_element/R`(N)();
  od:
  ok := `is_element/F`(N)(A)(x);
 od;

 return eval(x);
end;

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

`list_elements/F` := NULL;

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

`count_elements/F` := NULL;

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

`separation_infinity/F` := (N::posint) -> (A::set) -> proc(x)
 local x0,d,n,i,j;

 x0 := `normalise/F`(N)(A)(x);
 d := 2;
 n := nops(A);

 for i from 1 to n-1 do
  for j from i+1 to n do
   d := min(d,`d_infinity/R`(N)(x0[A[i]],x0[A[j]]));
  od;
 od;

 return d;
end;

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

`fatten/F` := (N::posint) -> (A::set) -> proc(x)
 local x0,d,m,a,i,e,u;

 x0 := `normalise/F`(N)(A)(x);
 d := `separation_infinity/F`(N)(A)(x0);
 m := 0;
 for a in A do
  for i from 1 to N do
   m := max(m,d/2+abs(x0[a][i]));
  od;
 od;

 e := [1 $ N];

 for a in A do
  u[a] := [((x0[a] -~ ((d/2) *~ e)) /~ m +~ e) /~ 2,
           ((x0[a] +~ ((d/2) *~ e)) /~ m +~ e) /~ 2];
 od;

 return eval(u);
end;

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

`inc/F/Fbar` := (N::posint) -> (A::set) -> proc(x)
 local y,P,T,t;

 y := table();

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

 for T in P do
  y[T] := table([seq(t = x[t],t in T)]);
 od:

 return eval(y);
end;

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

`gaps/F` := (N::posint) -> (A::set) -> proc(x)
 local G,V,W,m,i,j,a;

 G := NULL;
 m := nops(A);

 for i from 1 to N do
  V := sort([seq(x[a][i],a in A)]);
  W := [seq(V[j+1] - V[j],j = 1 .. m-1)];
  G := G,max(W);
 od;

 G := [G];
 return G;
end;

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

`gap/F` := (N::posint) -> (A::set) -> proc(x)
 return max(`gaps/F`(N)(A)(x));
end;

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

`normalise_gap/F` := (N::posint) -> (A::set) -> proc(x)
 local m,g,x0,a,i;

 m := [seq(min(seq(x[a][i],a in A)),i=1..N)];
 g := `gap/F`(N)(A)(x);

 x0 := table();
 for a in A do
  x0[a] := (x[a] -~ m) /~ g;
 od;

 return eval(x0);
end;

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

`delta/F` := (N::posint) -> (A::set) -> (a,b) -> proc(x)
 local u,n,i;
 u := x[b] -~ x[a];
 n := sqrt(add(u[i]^2,i=1..N));
 return u /~ n;
end:

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

`draw_unnormalised/F` := (A::set) -> proc(x)
 local a;
 display(seq(point(x[a]),a in A),scaling=constrained,axes=none);
end;

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

`draw/F` := (A::set) -> proc(x)
 display(`draw_unnormalised/F`(A)(`normalise/F`(2)(A)(x)),view=[-1..1,-1..1]);
end;


main()inline{
	HideTraceLog();
	ClearTraceLog();
	once do(MapHeight()){
		do(MapWidth())
			SetTerrain(i, j, (((sin((i+j)/10.0)+1)/2.0)*5.0 * ((cos((i-j)/10.0)+1.0)/2.0)*5.0 )/25.0 * 9+1);
		delay(1);
	}
}


# Kinematik-Berechnung 3. Arm KAS5 Modell 7
# Beschreibung
#
# Berechnung der kinematischen Zwangsbedingungen des Systems KAS5m7 in impliziter Form.
# Ansatz: Schließen von Gelenkketten nach [1] und Anwendung der Zahnrad-Zwangsbedingungen.
#
# Quellen
# [1] Khalil and Bennis: Symbolic calculation of the base inertial parameters of closed-loop robots (1995)
# Autor
# Moritz Schappler, schappler@irt.uni-hannover.de, 2018-02
# Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Initialisierung
restart:
kin_constraints_exist := true: # Für Speicherung
;
with(StringTools): # Für Zeitausgabe
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
codegen_act := true:
codegen_opt := 1: # Geringerer Optimierungsgrad. Sonst zu lange.
codegen_debug := 0: # Zur Code-Generierung auch für Nicht-Inert-Ausdrücke
;
read "../helper/proc_MatlabExport":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../robot_codegen_constraints/proc_subs_kintmp_exp":
read "../helper/proc_intersect_circle":
with(RealDomain): # Schränkt alle Funktionen auf den reellen Bereich ein. Muss nach Definition von MatlabExport kommen. Sonst geht dieses nicht.
;
read "../robot_codegen_definitions/robot_env_IC":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name_OL):
# Ergebnisse der Kinematik laden
read sprintf("../codeexport/%s/tmp/kinematics_floatb_%s_rotmat_maple.m", robot_name_OL, base_method_name);
read "../robot_codegen_definitions/robot_env_IC":
Trf := Trf:
Trf_c := Trf_c:
# Schleife über B-C-D
# 3-4-12-13-17
T_3_17 := combine( Matrix(Trf(1..4,1..4, 4)) . Matrix(Trf(1..4,1..4,12)) . Matrix(Trf(1..4,1..4,13)) . Matrix(Trf(1..4,1..4,17)) ):
# 3-10-11-20
T_3_20 := combine( Matrix(Trf(1..4,1..4, 10)) . Matrix(Trf(1..4,1..4,11)) . Matrix(Trf(1..4,1..4,20)) ):
h1 := T_3_17(1..3,4) - T_3_20(1..3,4):
# Schleife über 2-F-E
#
# 2-3-10-16
T_2_16 := combine( Matrix(Trf(1..4,1..4, 3)) . Matrix(Trf(1..4,1..4,10)) . Matrix(Trf(1..4,1..4,16)) ):
# 2-8-9-19
T_2_19 := combine( Matrix(Trf(1..4,1..4, 8)) . Matrix(Trf(1..4,1..4,9)) . Matrix(Trf(1..4,1..4,19)) ):
# Vektorkette
h2 := T_2_16(1..3,4) - T_2_19(1..3,4):
# Schleife über 5-B-A

# 4-5-6-18
T_4_18 := combine( Matrix(Trf(1..4,1..4, 5)) . Matrix(Trf(1..4,1..4,6)) . Matrix(Trf(1..4,1..4,18)) ):
# 4-12-13-14-15-21
T_4_21 := combine( Matrix(Trf(1..4,1..4, 12)) . Matrix(Trf(1..4,1..4,13)) . Matrix(Trf(1..4,1..4,14)) . Matrix(Trf(1..4,1..4,15)) . Matrix(Trf(1..4,1..4,21)) ):
# Vektorkette
h3 := T_4_18(1..3,4) - T_4_21(1..3,4):
# Zahnrad-Kontakt
#
hz1 := -qJ_t(4) + qJ_t(11):
hz2 := -qJ_t(6) + qJ_t(5) - qJ_t(4) + delta18:
h_user := <hz1; hz2>:
# Zusammenstellen aller Zwangsbedingungen
#
implconstr_t := <h1([1, 2],1);h2([1, 3],1);h3([1, 2],1); h_user>:
implconstr_s := convert_t_s(implconstr_t):
# Exportiere Code für folgende Skripte
#
#
kin_constraints_exist:=true:
save implconstr_t, implconstr_s, kin_constraints_exist, sprintf("../codeexport/%s/tmp/kinematic_constraints_implicit_maple.m", robot_name):
# Exportieren des vollständigen Ausdruckes
if codegen_act then
  MatlabExport(implconstr_s, sprintf("../codeexport/%s/tmp/kinconstr_impl_matlab.m", robot_name), 2):
end if:
# Liste mit abhängigen konstanten Kinematikparametern erstellen (wichtig für Matlab-Funktionsgenerierung)
read "../helper/proc_list_constant_expressions";
kc_symbols := Matrix(list_constant_expressions( implconstr_s )):
save kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_implicit_constraints_symbols_list_maple", robot_name):
MatlabExport(Transpose(kc_symbols), sprintf("../codeexport/%s/tmp/kinematic_implicit_constraints_symbols_list_matlab.m", robot_name), 2):
interface(rtablesize=100):
kc_symbols
;


# Energy Calculation for Atlas Robot based on MDH frames
# Introduction
# Berechnung von potentieller Energie für den Roboter.
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# tree -> Berechnung für eine beliebige Baumstruktur (ohne Schleifen)
# floatb -> floating base wird durch base twist (Geschwindigkeit der Basis) oder vollständige Orientierung (Euler-Winkel) berücksichtigt
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# energy -> Berechnung der Energie
# worldframe -> Berechnung der Positionen und Geschwindigkeiten im Welt-KS (KS W) anstatt im Basis-KS (KS 0)
# par2 -> Parametersatz 2 (erstes Moment anstelle von Schwerpunkt als Parameter)

# Anmerkung
# Nur potentielle Energie möglich
# Kinetische Energie stimmt nicht überein. Funktioniert nicht mit diesem Parametervektor und Geschwindigkeiten im Basis-KS
# Für die kinetische Energie sollte robot_tree_floatb_twist_rotmat_energy_linkframe_par2.mw genommen werden!
# Authors
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-03
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
#
# Sources
# [GautierKhalil1990] Direct Calculation of Minimum Set of Inertial Parameters of Serial Robots
# [KhalilDombre2002] Modeling, Identification and Control of Robots
# [Ortmaier2014] Vorlesungsskript Robotik I
# 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):
codegen_act := true:
codegen_opt := 2:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotx":
read "../transformation/proc_troty":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../transformation/proc_trafo_mdh":
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
read sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
kin_constraints_exist := kin_constraints_exist: # Laden zur Einstellung der Term-Vereinfachung
;
# Einstellungen für Term-Vereinfachungen: 0=keine, 1=E_pot(einzelne), 2=auch E_pot (komplett)
if not assigned(simplify_options) or simplify_options(6)=-1 then # Standard-Einstellungen:
  if not kin_constraints_exist then # normale serielle Ketten und Baumstrukturen
    use_simplify := 0: # Standardmäßig aus
  else # mit kinematischen Zwangsbedingungen
    use_simplify := 2: # standardmäßig alle simplify-Befehle anwenden
  end if:
else # Benutzer-Einstellungen:
  use_simplify := simplify_options(6): # sechster Eintrag ist für Energie
end if:
# Ergebnisse der Kinematik laden
read sprintf("../codeexport/%s/tmp/kinematics_floatb_%s_rotmat_maple.m", robot_name, base_method_name):
Trf_c := Trf_c:
read sprintf("../codeexport/%s/tmp/kinematics_com_worldframe_floatb_%s_par1_maple.m", robot_name, base_method_name):
r_W_i_Si := r_W_i_Si:
mr_W_i_Si := mr_W_i_Si:

# Potential Energy
U_b := Matrix(NL, 1):
U_grav := 0:
for i to NL do
  r_W_W_i := Matrix(3,1,Trf_c(1 .. 3, 4, i)):
  U_b[i] := -Multiply(Transpose(g_world), M[i, 1]*r_W_W_i+Matrix(3,1,mr_W_i_Si(1 .. 3, i))):
  printf("%s. Potentielle Energie aus Gravitation für Körper %d berechnet (im Welt-KS, mit Parametersatz 2).\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), i-1):#0=Basis
  # Terme vereinfachen (nur einzelne Energien und nicht die Summe.
  # Annahme: Zusammenfassung verschiedener Körper nicht zielführend
  if use_simplify>=1 then
    tmp_t1:=time(): tmp_l1 := length(U_b[i,1]):
    printf("%s. Beginne Vereinfachung für potentielle Energie (Param. 2; Körper %d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l1):
    U_b[i,1] := simplify2(U_b[i,1]):
    tmp_t2:=time(): tmp_l2 := length(U_b[i,1]):
    printf("%s: Terme für potentielle Energie (Param. 2) vereinfacht (Körper %d). Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end if:
  U_grav := U_grav+U_b[i, 1]:
  # Terme nochmal in Gesamtsumme zusammenfassen (zusätzlich)
  if use_simplify>=2 then
    tmp_t1:=time(): tmp_l1 := length(U_grav):
    printf("%s. Beginne Vereinfachung für potentielle Energie (Param. 2; Summe bis Körper %d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l1):
    U_grav := simplify2(U_grav):
    tmp_t2:=time(): tmp_l2 := length(U_grav):
    printf("%s: Terme für potentielle Energie (Param. 2) vereinfacht (Summe bis Körper %d). Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end if:
end do:
# Maple Export
save U_grav, sprintf("../codeexport/%s/tmp/energy_potential_floatb_%s_worldframe_par2_maple.m", robot_name, base_method_name):
# Kinetic Energy
# Using velocities in world frame and MX,MY,MZ as a parameter does not work.
# Only calculation of potential energy in this worksheet
# Matlab Export
printf("%s: Potentielle Energie (Param. 2) berechnet und gespeichert. Beginne Matlab-Export.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S")):
# Potential Energy
# Floating Base
U_s := convert_t_s(U_grav):
if codegen_act then
  MatlabExport(U_s, sprintf("../codeexport/%s/tmp/energy_potential_floatb_%s_worldframe_par2_matlab.m", robot_name, base_method_name), codegen_opt):
end if:
# Fixed Base
U_s_fixb:=U_s:
for i from 1 to NQB do
  U_s_fixb := subs({X_base_s[i,1]=0},U_s_fixb):
end do:
for i from 1 to 6 do
  U_s_fixb := subs({V_base_s[i,1]=0},U_s_fixb):
end do:
if codegen_act then
  MatlabExport(U_s_fixb, sprintf("../codeexport/%s/tmp/energy_potential_fixb_worldframe_par2_matlab.m", robot_name), codegen_opt):
end if:


(*
 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_lkt_params *params;

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

(* The m_min avoids divisions by zero *)
lkt_f0 := s -> 1/cosh(params_a_a * m_min(200, s)) + 5*s^2/3 :
lkt_f := x -> lkt_f0(X2S*x):

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


(*
 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: mgga_exc *)

lp90_c0 := 0.80569:
lp90_d0 := 3.0124e-3:
lp90_k  := 4.0743e-3:

(* Equation (60) *)
lp90_f := (rs, z, xt, us0, us1) ->
  - (lp90_c0 + lp90_d0*t_vw(z, xt, us0, us1))/(rs/RS_FACTOR + lp90_k):

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


# Berechnungen für Systeme mit kinematischen Zwangsbedingungen
# Beschreibung
# Dieses Arbeitsblatt ist für Systeme mit kinematischen Zwangsbedingungen relevant.
# Es werden die Zusammenhänge zwischen den Minimalkoordinaten des Systems mit Zwangsbedingungen mit dem offenen System ohne Zwangsbedingungen hergestellt (nach [NakamuraGho1989], [ParkChoPlo1999]).
# Das System ohne Zwangsbedingungen kann durch die Erstellung einer neuen robot_env definiert werden (Weglassen der Zwangsbedingungen, offene Baumstruktur).
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# kinematic_constraints_calculations -> Berechnungen bezüglich kinematischer Zwangsbedingungen
# Quellen
# [KhalilKle1986] Khalil, W. & Kleinfinger, J.: A new geometric notation for open and closed-loop robots (1986)
# [NakamuraGho1989] Nakamura, Yoshihiko and Ghodoussi, Modjtaba: Dynamics computation of closed-link robot mechanisms with nonredundant and redundant actuators (1989)
# [ParkChoPlo1999] Park, FC and Choi, Jihyeon and Ploen, SR: Symbolic formulation of closed chain dynamics in independent coordinates (1999)
# Autor
# Moritz Schappler, schappler@irt.uni-hannover.de, 2017-12
# Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Initialisierung
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
interface(rtablesize=30):
with(StringTools): # Für Zeitausgabe
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
codegen_act := true:
codegen_opt := 2: # Hoher Optimierungsgrad.
;
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
with(RealDomain): # Schränkt alle Funktionen auf den reellen Bereich ein. Muss nach Definition von MatlabExport kommen. Sonst geht dieses nicht.
;
read "../robot_codegen_constraints/proc_subs_kintmp_exp":
# Definition und Zwangsbedingeun
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
if not assigned(simplify_options) or simplify_options(1)=-1 then
  if NJ <= 5 then
    use_simplify := 1: # Nur bei sehr einfachen Systemen (Viergelenkkette). Sonst hängt es sich auf.
  else
    use_simplify := 0: # Sonst keine Vereinfachung.
  end if:
else
  use_simplify := simplify_options(1): # erster Eintrag ist für Zwangsbedingungen
end if:
# Lesen der Zwangsbedingungen
kin_constraints_exist := false:
constrfile := sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
if FileTools[Exists](constrfile) then
  read constrfile:
end if:
if kin_constraints_exist = true then:
  kintmp_qs := kintmp_qs: # gelesene Variable sonst nicht sichtbar
  kintmp_qt := kintmp_qt: # gelesene Variable sonst nicht sichtbar
  kintmp_subsexp := kintmp_subsexp: # gelesene Variable sonst nicht sichtbar
  printf("%s. Kinematische Zwangsbedingungen gelesen.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
else
  printf("%s. Es gibt keine Zwangsbedingungen. Offene Struktur. Keine weiteren Berechnungen notwendig.", FormatTime("%Y-%m-%d %H:%M:%S")):
  restart: # Funktioniert nicht. Gedacht, damit im worksheet-Modus nichts mehr passieren kann.
  robot_name := "": # Damit werden die Export-Befehle ungültig
  codegen_act := false:
  quit:
end if:
# Abhängige Koordinaten definieren
# Nicht alle Ersetzungsausdrücke sind auch Koordinaten
NSE := RowDimension(kintmp_s):
# temporäre kinematische Ausdrücke (die auch die Zwangsbedingungen enthalten)
kintmp_s:
# Gelenkvariable für Drehgelenke (nach [KhalilKle1986])
theta_s := convert_t_s(theta):
# Gelenkvariable für Schubgelenke (nach [KhalilKle1986])
d_s := convert_t_s(d):
# Finde und zähle die Vorkommnisse von kintmp_s in theta_s und d_s. Die Anzahl der abhängigen Gelenkvariablen muss mit den definierten Minimalkoordinaten übereinstimmen, sonst ist das benutzerspezifizierte Modell falsch.
# Zähle, welche Zwangsbedingungen tatsächlich vorkommen
kintmp_zaehler := Matrix(NSE,1):
for i from 1 to NSE do
  if has(has~(<theta_s(1..NJ,1);d_s(1..NJ,1)>, kintmp_s(i,1)), true) then
    kintmp_zaehler(i,1) := 1:
  end if:
end do:
# Zähle, welche Gelenke abhängig sind (durch ZB beschrieben).
# (in der Gelenkvariablen stehen die Inhalte von kintmp)
jv_zaehler := Matrix(NJ,1):
for i from 1 to NJ do
  if has(has~(theta_s(i,1), kintmp_s), true) or has(has~(d_s(i,1), kintmp_s), true) then
    jv_zaehler(i,1) := 1:
  end if:
end do:
# Zählen, welche Gelenke statische Transformationen sind
sigma2_zaehler := Matrix(NJ,1):
for i from 1 to NJ do
  if sigma(i,1)=2 then
    sigma2_zaehler(i,1) := 1:
  end if:
end do:
# Anzahl der abhängigen Gelenkvariablen (Zählmethode: Gelenkkoordinaten durchgehen)
NC1 := add(jv_zaehler(n), n = 1 .. NJ):
# Anzahl der abhängigen Gelenkvariablen (Zählmethode: Zwangsbedingungen durchgehen)
NC2 := add(kintmp_zaehler(n), n = 1 .. NSE):
# Anzahl der konstanten Transformationen
NS := add(sigma2_zaehler(n), n = 1 .. NJ):
# Anzahl der "realen" Gelenke (statische Transformationen herausrechnen)
NRJ := NJ - NS:
# Es gibt (NL - 1) durch Gelenke bewegte Körper, NVJ/2 Koordinatensysteme (hier als "Gelenk" gezählt) sind statisch aufgrund der Schleifenschlussbedingungen
if NQJ+NC1 <> NRJ then
  printf("%s. Fehler: Anzahl der Abhängigen Gelenke %d (aus Gelenkvariablen) passt nicht zur Anzahl der Gelenke %d und der Minimalkoordinaten %d\n", FormatTime("%Y-%m-%d %H:%M:%S"), NC1, NRJ, NQJ):
end if:
if NC1 <> NC2 then
  printf("%s. Fehler: Anzahl der Abhängigen Gelenke %d (aus Gelenkvariablen) passt nicht zur Anzahl der Zwangsbedingungen %d (aus Verwendung der Zwangsbedingungs-Variablen)\n", FormatTime("%Y-%m-%d %H:%M:%S"), NC1, NC2):
end if:
# Indizes der Zwangsbedingungen (in kintmp_s), die wirklich verwendet werden. Wird später nicht mehr benötigt.
Ind_ZB := Matrix(NC,1):
j := 1:
for i from 1 to NSE do
  if kintmp_zaehler(i,1) = 1 then
    Ind_ZB(j,1) := i:
    j := j + 1:
  end if:
end do:
# Koordinaten der offenen Struktur (MDH-Gelenkvariablen)
# Darstellung der Gelenkvariablen in abhängigkeit der Minimalkoordinaten. Diese Gelenkvariablen sind die Minimalkoordinaten des offenen Systems (ohne Zwangsbedingungen)
jv_s := theta_s*~(Matrix(NJ,1,1)-sigma) + d_s*~sigma - qoffset:
jv_qs := jv_s: # Initialisierung
jv_qt := Matrix(NJ,1): # Initialisierung
;
# Ersetzen der abhängigen Gelenkkoordinaten
for i from 1 to NJ do # Index über alle Gelenkvariablen
  # Substituiere sin und cos der Winkel (einfachere Ausdrücke. Sollten aber hier nicht drin sein.)
  jv_qs(i,1) := subs_kintmp_exp(jv_qs(i,1)):
  # Substituiere die verbleibenden Winkel direkt
  for jj from 1 to NSE do # Index über zu ersetzende Winkel
    jv_qs(i,1) := subs( { kintmp_s(jj, 1) = kintmp_qs(jj, 1) }, jv_qs(i,1) ):
  end do:
  jv_qt(i,1) := convert_s_t( jv_qs(i,1) ):
end do:
# Jacobi-Matrix der abhängigen Winkel
# Ableitung der Gelenkkoordinaten nach den Minimalkoordinaten.
# Wird benötigt, um die Geschwindigkeit der Gelenkkoordinaten der offenen Struktur zu berechnen und um die Momente der offenen Struktur auf die geschlossene Struktur umzurechnen.
# Ist Matrix Phi aus [ParkChoPlo1999]. qJ hier ist qa dort. qr dort entspricht den Koordinaten desselben Systems ohne Zwangsbedingungen.
# Entspricht der Matrix W aus [NakamuraGho1989] (nur dass hier die Aufteilung q1,q2 nicht gestapelt in q auftreten muss).
# Wenn statische Transformationen definiert sind (mit sigma=2), dann werden die entsprechenden Spalten hier trotzdem angegeben (das ist nicht unbedingt intuitiv, aber von der Programmierung bedeutend einfach)
Phi_s := Matrix(NJ, NQJ):
# Jacobi-Matrix berechnen
for i from 1 to NJ do
  for j from 1 to NQJ do
    Phi_s(i,j) := diff(jv_qs(i,1), qJ_s(j,1)):
  end do:
end do:
# Term vereinfachen
if use_simplify >= 1 then
  tmp_t1 := time(): tmp_l1 := length(Phi_s):
  printf("%s. Vereinfache Passiv-Gelenk-Jacobi (Dimension %d x %d). Länge: %d.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), NJ, NQJ, tmp_l1):
  for i from 1 to NJ do
    for j from 1 to NQJ do
      tmp_t1ij := time(): tmp_l1ij := length(Phi_s(i,j)):
      printf("%s. Vereinfache Passiv-Gelenk-Jacobi Eintrag %d,%d. Länge: %d.\n", \
        FormatTime("%Y-%m-%d %H:%M:%S"), i, j, tmp_l1ij):
      Phi_s(i,j) := simplify2(Phi_s(i,j)):
      tmp_t2ij := time(): tmp_l2ij := length(Phi_s(i,j)):
      printf("%s. Passiv-Gelenk-Jacobi der ZB Eintrag %d,%d vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
        FormatTime("%Y-%m-%d %H:%M:%S"), i, j, tmp_l1ij, tmp_l2ij, tmp_t2ij-tmp_t1ij):
    end do:
  end do:
  tmp_t2 := time(): tmp_l2 := length(Phi_s):
  printf("%s. Passiv-Gelenk-Jacobi der ZB vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), tmp_l1, tmp_l2, tmp_t2-tmp_t1):
end if:
# Zeitableitung der Jacobi-Matrix
# Wird benötigt, um die Beschleunigung der Gelenkkoordinaten der offenen Struktur zu berechnen.
Phi_t := convert_s_t(Phi_s):
PhiD_t := diff~(Phi_t, t):
PhiD_s := convert_t_s(PhiD_t):
# Exportiere Code für folgende Skripte
# Speichere Maple-Ausdruck (Eingabe-Format und internes Format)
save jv_s, jv_qs, jv_qt, sprintf("../codeexport/%s/tmp/kinematic_constraints_explicit_maple.m", robot_name):
save Phi_s, sprintf("../codeexport/%s/tmp/kinematic_constraints_explicit_jacobian_maple.m", robot_name):
save PhiD_s, sprintf("../codeexport/%s/tmp/kinematic_constraints_explicit_jacobian_time_derivative_maple.m", robot_name):
printf("%s. Ausdrücke für Kinematische ZB gespeichert (Maple)\n", FormatTime("%Y-%m-%d %H:%M:%S")):
if codegen_act = true then
  MatlabExport(jv_zaehler, sprintf("../codeexport/%s/tmp/kinconstr_index_dependant_joints_matlab.m", robot_name), 2):
  MatlabExport(jv_qs, sprintf("../codeexport/%s/tmp/kinconstr_expl_matlab.m", robot_name), 2):
  MatlabExport(Phi_s, sprintf("../codeexport/%s/tmp/kinconstr_expl_jacobian_matlab.m", robot_name), 2):
  MatlabExport(PhiD_s, sprintf("../codeexport/%s/tmp/kinconstr_expl_jacobianD_matlab.m", robot_name), 2):
  printf("%s. Ausdrücke für Kinematische ZB gespeichert (Matlab)\n", FormatTime("%Y-%m-%d %H:%M:%S")):
end if:


read "../IdentifiabilityODE.mpl";

sys := [
diff(x2(t), t) = alpha*x1(t) - beta*x2(t),
diff(x4(t), t) = (gama*sigma*x2(t)*x4(t) - delta*sigma*x3(t)*x4(t)) / (x3(t)),
diff(x1(t), t) = (-b*c*x1(t) - b*x1(t)*x4(t) + 1) / (c + x4(t)),
diff(x3(t), t) = gama*x2(t) - delta*x3(t),
y(t) = x1(t)
];
CodeTools[CPUTime](IdentifiabilityODE(sys, GetParameters(sys)));


# Plotting

Plotting := module()

  description "plotting";

  # Module defined as a package (i.e.) collection of procedures
  option package, load = ModuleLoad, unload = ModuleUnLoad;

  export plot_log, plot_iter;

  local ModuleLoad, ModuleUnLoad,
        EPSI, LLL, COLORS, CONTOURPARS;

  uses plots;

  ModuleUnLoad := proc()
    NULL;
  end proc:

  ModuleLoad := proc()
    NULL;
    EPSI        := 1e-50;
    LLL         := thickness=2,style=pointline;
    COLORS      := [ "blue", "ForestGreen", "black", "DeepPink", "Goldenrod" ];
    CONTOURPARS := filledregions = true, contours=50,
                   coloring = ["LimeGreen", "Goldenrod"],
                   transparency = 0.5;
  end proc;

  ModuleLoad();

  plot_log := proc( V_list )
    local i, iii, V, A, A_list;
    A_list := [];
    iii    := 1;
    for V in V_list do
      A      := logplot( [seq([i,abs(V[i])+EPSI],i=1..nops(V))],
                         LLL, color=COLORS[iii] );
      A_list := [op(A_list), A];
      iii    := iii+1;
    end;
    display(op(A_list));
  end proc:

  plot_iter := proc( epxr, XRANGE, YRANGE, X_list )
    local i, iii, X, AA, A, A_list:
    AA := contourplot( epxr, XRANGE, YRANGE, CONTOURPARS ):
    iii := 1;
    A_list := [];
    for X in X_list do
      A := plot( [seq([X[i][1],X[i][2]],i=1..nops(X))],
                  LLL, color=COLORS[iii] ):
      A_list := [op(A_list), A];
      iii    := iii+1;
      if iii > 4 then
        iii := 1;
      end;
    end;
    display( AA, op(A_list), scaling=constrained );
  end proc:

end module:


# Recall that $\Lambda^*$ is defined to be the monoid of lists of
# finite sets of natural numbers.  Such a list can be represented
# in Maple like [{1,4,6},{2,3,4,5},{1}], for example.  Similarly,
# if $N$ is a finite set of natural numbers, then $\Lambda^*[N]$
# is the submonoid generated by the subses of $N$.

# The function `is_element/itloc`(N)(AA) will return true iff
# AA is an element of $\Lambda^*[N]$.

`is_element/itloc` := (N::set) -> proc(AA)
 return type(AA,list(set(nonnegint)));
end:

# The function `is_small_element/itloc`(N)(l)(AA) will return true
# iff AA is an element of $\Lambda^*[N]$ and has length $l$.

`is_small_element/itloc` := (N::set) -> (l::nonnegint) -> proc(AA)
 local A;

 if not(`is_element/itloc`(N)(AA)) then
  return false;
 fi;

 if nops(AA) > l then return false; fi;
 A := map(op,{op(AA)});
 if A minus N <> {} then return false; fi;
 return true;
end:

# The function `list_small_elements/itloc`(N)(l) will produce a
# list of all the elements of $\Lambda^*[N]$ that have length l.
# (This will obviously be unmanageably large unless N and l are
# very small.)

`list_small_elements/itloc` := (N::set) -> proc(l::nonnegint)
 option remember;
 local PN,L,i;

 PN := combinat[powerset](N);
 if l = 0 then
  return [[]];
 else
  L := `list_small_elements/itloc`(N)(l-1);
  return [seq(seq([op(AA),A],A in PN),AA in L)];
 fi;
end:

# The function `list_smaller_elements/itloc`(N)(l) lists
# elements of length <= l.

`list_smaller_elements/itloc` := (N::set) -> proc(l::nonnegint)
 [seq(op(`list_small_elements/itloc`(N)(k)),k=0..l)];
end:

# The function `count_small_elements/itloc`(N)(l) returns
# the number of elements of length l.

`count_small_elements/itloc` := (N::set) -> (l::nonnegint) ->
 2^(nops(N) * l);

# The function `random_small_element/itloc`(N)(l)() returns
# a randomly chosen element of $\Lambda^*[N]$ of length l.
# (Note the extra pair of brackets in the syntax.)

`random_small_element/itloc` := (N::set) -> (l::nonnegint) -> proc()
 local r;

 r := `random_element/subsets`(N);
 return [seq(r(),i=1..l)];
end:

# The function `o/itloc` is the multiplication law for the
# monoid $\Lambda^*$: so for example `o/itloc`(AA,BB,CC) is
# just the list obtained by concatenating AA, BB and CC.

`o/itloc` := () -> map(op,[args]);

# `id/itloc` is the empty list, which is the identity element
# of the monoid $\Lambda^*$.

`id/itloc` := [];

# `omega/itloc`(AA) is the union of all the sets in the list AA.
# `cap/itloc`(AA,B) is the sequence of intersections of sets in
# AA with the fixed set B. (This is defn-om in the LaTeX file.)

`omega/itloc` := (AA) -> map(op,{op(AA)});
`cap/itloc` := (AA,B) -> map(A -> `intersect`(A,B),AA);

# `sigma/itloc`(N)(AA) is the intersection of all the sets in the
# list AA, to be interpreted as N if the list AA is empty.
# This is essentially the same as in rem-sg-xi in the LaTeX file.

`sigma/itloc` := (N::set) -> (AA) -> `intersect`(N,op(AA));

# `prec/itloc`(A,B) returns true if every element of A is strictly
# less than every element of B.  The function `preceq/itloc`(A,B)
# returns true if every element of A is less than or equal to
# every element of B.  These are in defn-prec in the LaTeX file.
`prec/itloc` := proc(A::set,B::set)
 if A = {} or B = {} then return true; fi;
 return evalb(max(op(A)) < min(op(B)));
end:

`preceq/itloc` := proc(A::set,B::set)
 if A = {} or B = {} then return true; fi;
 return evalb(max(op(A)) <= min(op(B)));
end:

# The next five definitions correspond to defn-reduced in the LaTeX file.

`is_top_reduced/itloc` := proc(AA)
 local t;
 t := map(A -> max(-infinity,op(A)),AA);
 return evalb(t = sort(t));
end:

`is_bottom_reduced/itloc` := proc(AA)
 local b;
 b := map(A -> min(op(A),infinity),AA);
 return evalb(b = sort(b));
end:

`is_unnested/itloc` := proc(AA)
 local r,i;
 r := nops(AA);
 for i from 1 to r-1 do
  if AA[i] minus AA[i+1] = {} then
   return false;
  fi;
  if AA[i+1] minus AA[i] = {} then
   return false;
  fi;
 od;
 return true;
end:

`is_end_reduced/itloc` := proc(AA)
 `is_top_reduced/itloc`(AA) and
 `is_bottom_reduced/itloc`(AA);
end:

`is_reduced/itloc` := proc(AA)
 `is_top_reduced/itloc`(AA) and
 `is_bottom_reduced/itloc`(AA) and
 `is_unnested/itloc`(AA);
end:

# The next three definitions correspond to defn-reductions and
# lem-trimming-confluent
`beta/itloc` := proc(AA)
 local r,i,n,BB;

 r := nops(AA);
 if r = 0 then return []; fi;
 BB := table();
 BB[1] := AA[1];
 for i from 2 to r do
  if BB[i-1] = [] then
   return [[]];
  fi;
  n := min(BB[i-1]);
  BB[i] := select(j -> evalb(j >= n),AA[i]);
 od;
 return [seq(BB[i],i=1..r)];
end:

`tau/itloc` := proc(AA)
 local r,i,n,BB;

 r := nops(AA);
 if r = 0 then return []; fi;
 BB := table();
 BB[r] := AA[r];
 for i from r-1 to 1 by -1 do
  if BB[i+1] = [] then
   return [[]];
  fi;
  n := max(BB[i+1]);
  BB[i] := select(j -> evalb(j <= n),AA[i]);
 od;
 return [seq(BB[i],i=1..r)];
end:

`delta/itloc` := proc(AA)
 local r,m,i,BB,CC,Bi,Cm,U,V,ok;

 r := nops(AA);
 if r = 0 then return []; fi;

 ok := false;
 CC := AA;
 while not(ok) do
  BB := CC;
  r := nops(BB);
  CC := [BB[1]];
  m := 1;
  for i from 2 to r do
   Bi := {op(BB[i])};
   Cm := {op(CC[m])};
   U := Bi minus Cm;
   V := Cm minus Bi;
   if U = {} then
    CC := [seq(CC[j],j=1..m-1),BB[i]];
   elif V <> {} then
    CC := [op(CC),BB[i]];
    m := m+1;
   fi;
  od:
  ok := evalb(CC = BB);
 od;

 return BB;
end:

`end_reduce/itloc` := (AA) -> `tau/itloc`(`beta/itloc`(AA)):

`reduce/itloc` := (AA) -> `delta/itloc`(`tau/itloc`(`beta/itloc`(AA))):

`rho/itloc` := eval(`reduce/itloc`):

# `oo/itloc` concatenates its arguments, and then applies the
# reduction map.

`oo/itloc` := proc()
 `reduce/itloc`(`o/itloc`(args));
end:

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

# The set $\Theta(AA)$ of threads is defined in defn-threads.
# The function `is_element/threads/itloc`(AA)(a) returns true
# iff a is a thread for AA.

`is_element/threads/itloc` := (AA) -> proc(a)
 local r,i;

 r := nops(AA);
 if not(type(a,list) and nops(a) = r) then
  return false;
 fi;

 for i from 1 to r do
  if not(member(a[i],AA[i])) then
   return false;
  fi;
 od;

 if a <> sort(a) then
  return false;
 fi;

 return true;
end:

# The function `is_leq/threads/itloc`(AA)(a,b) returns true iff
# a[i] <= b[i] for all i.

`is_leq/threads/itloc` := (AA) -> proc(a,b)
 local i;
 for i from 1 to nops(AA) do
  if a[i] > b[i] then return false; fi;
 od;
 return true;
end:

# The function `list_elements/threads/itloc`(AA) returns a list
# of all the threads for AA.

`list_elements/threads/itloc` := proc(AA)
 local r,AA0,L0,L,U,a0;

 r := nops(AA);

 if r = 0 then
  return [[]];
 elif r = 1 then
  return [op(map(a -> [a],AA[1]))];
 fi;

 AA0 := [seq(AA[i],i=1..r-1)];
 L0 := `list_elements/threads/itloc`(AA0);
 L := NULL;
 for a0 in L0 do
  U := select(a -> a0[r-1] <= a,AA[r]);
  L := L,seq([op(a0),a],a in U);
 od:
 return [L];
end:

# The function `random_element/threads`(AA)() attempts to return
# a randomly chosen thread for AA.  It is not very intelligent,
# so it may fail to find a thread even if one exists.  It will
# return the symbol FAIL if it does not find a thread.

`random_element/threads/itloc` := (AA) -> proc(num_tries::posint := 10)
 local k,n,a,i,B;

 if min(op(map(nops,AA))) = 0 then return FAIL; fi;

 k := num_tries;
 n := nops(AA);

 while k > 0 do
  k := k-1;
  a := [random_element_of(AA[1])];
  for i from 2 to n do
   B := select(b -> b >= a[-1],AA[i]);
   if B = {} then
    break;
   else
    a := [op(a),random_element_of(B)];
   fi;
  od;
  if nops(a) = n then return a; fi;
 od;

 return FAIL;
end:

# We do not know an efficient method to compute the number of threads,
# so we set `count_elements/threads/itloc` to NULL.
`count_elements/threads/itloc` := NULL:

# If the poset of threads is nonempty then it will have a smallest
# element, which will be returned by the function
# `bot/threads/itloc`(AA).

`bot/threads/itloc` := proc(AA)
 local BB;
 BB := `beta/itloc`(AA);
 if member({},BB) then
  return FAIL;
 else
  return map(min,BB);
 fi;
end:

# If the poset of threads is nonempty then it will have a largest
# element, which will be returned by the function
# `top/threads/itloc`(AA).

`top/threads/itloc` := proc(AA)
 local CC;
 CC := `tau/itloc`(AA);
 if member({},CC) then
  return FAIL;
 else
  return map(max,BB);
 fi;
end:

# `is_threadable/itloc`(AA) returns true iff AA has a thread
`is_threadable/itloc` :=
 (AA) -> (`bot/threads/itloc`(AA) <> FAIL);

# `min_thread_sets/itloc`(AA) returns the minimal elements of the
# poset of thread sets for AA (as in defn-threads)
`min_thread_sets/itloc` := proc(AA)
 local U,V,W,a,b;
 U := {op(`list_elements/threads/itloc`(AA))};
 U := map(u -> {op(u)},U);
 U := [op(U)];
 U := sort(U,(a,b) -> evalb(nops(a) < nops(b)));
 V := [];
 for a in U do
  W := select(b -> b minus a = {},V);
  if W = [] then V := [op(V),a]; fi;
 od;
 return {op(V)};
end:

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

# $\Phi_0[N]$ is the submonoid of $\Lambda[N]$ generated by the
# functors $L_{K(n)}$.  It bijects naturally with the subset of
# $\Lambda^*[N]$ consisting of the zero element [{}], together
# with all elements [{a[1]},...,{a[r]}] with a[1] <= ... <= a[r].

`is_element/Phi0/itloc` := (N::set(nonnegint)) -> proc(AA)
 local P;

 if not(`is_element/itloc`(N)(AA)) then
  return false;
 fi;

 if AA = [{}] then return true; fi;

 if {1,op(map(nops,AA))} <> {1} then
  return false; # not all singletons
 fi;

 P := map(op,AA);
 if sort(P) <> P then
  return false;
 fi;

 return true;
end:

`is_leq/Phi0/itloc` := (N::set(nonnegint)) -> proc(AA,BB)
 local P,Q;

 if AA = [{}] then return true; fi;
 if BB = [{}] then return false; fi;
 P := {op(map(op,AA))};
 Q := {op(map(op,AA))};

 return evalb(P minus Q = {});
end:

`list_elements/Phi0/itloc` := proc(N::set(nonnegint))
 local L;
 L := combinat[choose](sort([op(N)]));
 L := map(P -> map(i -> {i},P),L);
 L := [[{}],op(L)];
 return L;
end:

`random_element/Phi0/itloc` := proc(N::set(nonnegint))
 local P,AA;

 if rand(1..6)() = 1 then
  return [{}];
 else
  P := sort([op(random_subset_of(N)())]);
  AA := map(u -> {u},P);
  return AA;
 fi;
end:

`count_elements/Phi0/itloc` := proc(N::set(nonnegint))
 1 + 2^nops(N);
end:

`o/Phi0/itloc` := proc()
 local AA,P;

 if member([{}],[args]) then
  return [{}];
 fi;

 AA := map(op,[args]);
 P := map(op,AA);
 if P <> sort(P) then
  return [{}];
 fi;

 return AA;
end:

`id/Phi0/itloc` := [];

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

# M11/itloc(N)(A) is the set $M'_{N,A}$ from defn-M-prime

`is_element/M11/itloc` := (N::set(nonnegint)) -> (A::set(nonnegint)) -> proc(T)
 evalb(type(T,set) and (T minus N = {}) and (T intersect A <> {}));
end:

`is_leq/M11/itloc` := (N) -> (A) -> (T0,T1) -> evalb(T0 minus T1 = {}):

`list_elements/M11/itloc` := (N::set(nonnegint)) -> proc(A::set(nonnegint))
 return [op(select(T -> T intersect A <> {},combinat[powerset](N)))];
end:

`random_element/M11/itloc` := (N::set(nonnegint)) -> (A::set(nonnegint)) -> proc()
 local U,V;

 U := `random_element/nonempty_subsets`(A)();
 V := `random_element/subsets`(N minus A)();
 return U union V;
end:

`count_elements/M11/itloc` := (N::set(nonnegint)) -> proc(A::set(nonnegint))
 2^nops(N) - 2^nops(N minus A);
end:

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

# M1/itloc(N)(AA) is the set $M'_{N,AA}$ from defn-M-prime-multi

`is_element/M1/itloc` := (N::set(nonnegint)) -> (AA) -> proc(TT)
 local r,i;

 r := nops(AA);

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

 if nops(TT) <> r then
  return false;
 fi;

 if map(op,{op(TT)}) minus N <> {} then
  return false;
 fi;

 for i from 1 to r do
  if TT[i] intersect AA[i] = {} then
   return false;
  fi;
 od;

 for i from 1 to r-1 do
  if not(`preceq/itloc`(TT[i],TT[i+1])) then
   return false;
  fi;
 od;

 return true;
end:

`is_leq/M1/itloc` := (N) -> (AA) -> proc(TT0,TT1)
 `and`(seq(TT0[i] minus TT1[i] = {},i=1..nops(TT0)));
end:

`list_elements/M1/itloc` := (N::set(nonnegint)) -> proc(AA)
 local r,m,L,L0,L1,L2,AA0,TT0;
 r := nops(AA);
 if r = 0 then
  return [[]];
 fi;

 L1 := `list_elements/M11/itloc`(N)(AA[r]);

 if r = 1 then
  return map(T -> [T],L1);
 fi;

 AA0 := [seq(AA[i],i=1..r-1)];
 L0 := `list_elements/M1/itloc`(N)(AA0);
 L := NULL;
 for TT0 in L0 do
  m := max(TT0[r-1]);
  L2 := select(T -> min(T) >= m,L1);
  L := L,seq([op(TT0),T],T in L2);
 od:
 L := [L];
 return L;
end:

`random_element/M1/itloc` := (N::set(nonnegint)) -> (AA) -> proc(num_tries := 10)
 local r,k,AA0,TT0,m,N0,A0,T;
 r := nops(AA);
 if r = 0 then
  return [];
 fi;

 k := num_tries;
 while k > 0 do
  k := k-1;
  AA0 := [seq(AA[i],i=1..r-1)];
  TT0 := `random_element/M1/itloc`(N)(AA0)(num_tries);
  if TT0 <> FAIL then
   N0 := N;
   if r > 1 then
    m := max(op(TT0[r-1]));
    N0 := select(i -> (i >= m),N);
   fi;
   A0 := N0 intersect AA[r];
   if A0 <> {} then
    T := `random_element/M11/itloc`(N0)(A0)();
    if T <> FAIL then
     return [op(TT0),T];
    fi;
   fi;
  fi;
 od;

 return FAIL;
end:

`count_elements/M1/itloc` := NULL:

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

`is_element/M2/itloc` := (N::set(nonnegint)) -> (AA) -> proc(T::set(nonnegint))
 `is_threadable/itloc`(`cap/itloc`(AA,T)) and
 (T minus N) = {};
end:

`is_leq/M2/itloc` := (N::set(nonnegint)) -> (AA) -> proc(T0,T1)
 evalb(T0 minus T1 = {});
end:

`list_elements/M2/itloc` := (N::set(nonnegint)) -> proc(AA)
 local L;

 L := `list_elements/threads/itloc`(AA);
 L := map(a -> {op(a)},L);
 return [seq(seq(T union U,U in combinat[powerset](N minus T)),T in L)];
end:

`random_element/M2/itloc` := (N::set(nonnegint)) -> (AA) -> proc()
 local a,T;
 a := `random_element/threads/itloc`(AA)();
 if a = FAIL then return FAIL; fi;
 T := {op(a)};
 T := {op(T),op(random_subset_of(N minus T))};
 return T;
end:

`count_elements/M2/itloc` := NULL:

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

`is_element/M3/itloc` := (N::set(nonnegint)) -> (AA) -> (T) -> proc(TT)
 `is_element/M1/itloc`(N)(AA)(TT) and
  `omega/itloc`(TT) minus T = {};
end:

`is_leq/M3/itloc` := (N::set(nonnegint)) -> (AA) -> (T) -> proc(TT0,TT1)
 `and`(seq(evalb(TT0[i] minus TT1[i] = {}),i=1..nops(TT0)));
end:

`list_elements/M3/itloc` := (N::set(nonnegint)) -> (AA) -> proc(T)
 local BB,L;

 BB := `cap/itloc`(AA,T);
 L := `list_elements/M1/itloc`(T)(BB);
 return L;
end:

`phi/itloc` := (N::set(nonnegint)) -> (AA) -> (T) -> (TT) -> proc(k)
 local r,i,BB,b;
 r := nops(AA);
 if k < 0 or k > 2*r then error("k is out of range"); fi;
 i := ceil(k/2);
 BB := `beta/itloc`(`cap/itloc`(AA,T));
 if member({},BB) then error("T is not a thread set for AA"); fi;
 b := map(min,BB);
 if k = 0 then
  return TT;
 elif k = 2*i then
  return [seq({b[j]},j=1..i),seq(TT[j],j=i+1..r)];
 else
  return [seq({b[j]},j=1..i-1),TT[i] union {b[i]},seq(TT[j],j=i+1..r)];
 fi;
end:


read "../IdentifiabilityODE.mpl";

sys := [
diff(KS01(t), t) = c0001*KS00(t) + FS11(t)*gamma1101 - F(t)*alpha01*S01(t) + FS01(t)*beta01 - K(t)*S01(t)*a01 + KS01(t)*b01,
diff(FS10(t), t) = -c0001*KS00(t) - b00*KS00(t) + S00(t)*K(t)*a00 - c0011*KS00(t) - c0010*KS00(t),
diff(S11(t), t) = FS11(t)*beta11 + FS11(t)*gamma1100 + FS11(t)*gamma1101 + FS11(t)*gamma1110 - F(t)*alpha10*S10(t) - F(t)*alpha11*S11(t) - F(t)*alpha01*S01(t) + gamma1000*FS10(t) + FS01(t)*beta01 + FS01(t)*gamma0100 + beta10*FS10(t),
diff(S00(t), t) = F(t)*alpha10*S10(t) - gamma1000*FS10(t) - beta10*FS10(t),
diff(S10(t), t) = c0001*KS00(t) - a10*K(t)*S10(t) + KS10(t)*b10 + KS10(t)*c1011 + b00*KS00(t) + c0111*KS01(t) - S00(t)*K(t)*a00 - K(t)*S01(t)*a01 + c0011*KS00(t) + c0010*KS00(t) + KS01(t)*b01,
diff(F(t), t) = F(t)*alpha01*S01(t) - FS01(t)*beta01 - FS01(t)*gamma0100,
diff(KS00(t), t) = FS11(t)*gamma1100 + gamma1000*FS10(t) + FS01(t)*gamma0100 + b00*KS00(t) - S00(t)*K(t)*a00,
diff(S01(t), t) = -FS11(t)*beta11 - FS11(t)*gamma1100 - FS11(t)*gamma1101 - FS11(t)*gamma1110 + F(t)*alpha11*S11(t),
diff(KS10(t), t) = -a10*K(t)*S10(t) + FS11(t)*gamma1110 - F(t)*alpha10*S10(t) + KS10(t)*b10 + beta10*FS10(t) + c0010*KS00(t),
diff(FS01(t), t) = FS11(t)*beta11 - F(t)*alpha11*S11(t) + KS10(t)*c1011 + c0111*KS01(t) + c0011*KS00(t),
diff(FS11(t), t) = -c0111*KS01(t) + K(t)*S01(t)*a01 - KS01(t)*b01,
diff(K(t), t) = a10*K(t)*S10(t) - KS10(t)*b10 - KS10(t)*c1011,
y1(t) = F(t),
y5(t) = S11(t),
y3(t) = S01(t),
y0(t) = K(t),
y4(t) = S10(t),
y2(t) = S00(t)
];
CodeTools[CPUTime](IdentifiabilityODE(sys, GetParameters(sys)));


with(Groebner):
with(PolynomialIdeals):

J := PolynomialIdeal({10*x1*x2^2 + 10*x1*x3^2 + 10*x1*x4^2 + 10*x1*x5^2 + 10*x1*x6^2 + 10*x1*x7^2 - 11*x1 + 10, 10*x1^2*x2 + 10*x2*x3^2 + 10*x2*x4^2 + 10*x2*x5^2 + 10*x2*x6^2 + 10*x2*x7^2 - 11*x2 + 10, 10*x1^2*x3 + 10*x2^2*x3 + 10*x3*x4^2 + 10*x3*x5^2 + 10*x3*x6^2 + 10*x3*x7^2 - 11*x3 + 10, 10*x1^2*x4 + 10*x2^2*x4 + 10*x3^2*x4 + 10*x4*x5^2 + 10*x4*x6^2 + 10*x4*x7^2 - 11*x4 + 10, 10*x1^2*x5 + 10*x2^2*x5 + 10*x3^2*x5 + 10*x4^2*x5 + 10*x5*x6^2 + 10*x5*x7^2 - 11*x5 + 10, 10*x1^2*x6 + 10*x2^2*x6 + 10*x3^2*x6 + 10*x4^2*x6 + 10*x5^2*x6 + 10*x6*x7^2 - 11*x6 + 10, 10*x1^2*x7 + 10*x2^2*x7 + 10*x3^2*x7 + 10*x4^2*x7 + 10*x5^2*x7 + 10*x6^2*x7 - 11*x7 + 10}):
st := time[real]():
Basis(J, tdeg(x1,x2,x3,x4,x5,x6,x7), charactesistic=2^31-1):
time[real]() - st;

J := PolynomialIdeal({10*x1*x2^2 + 10*x1*x3^2 + 10*x1*x4^2 + 10*x1*x5^2 + 10*x1*x6^2 + 10*x1*x7^2 + 10*x1*x8^2 - 11*x1 + 10, 10*x1^2*x2 + 10*x2*x3^2 + 10*x2*x4^2 + 10*x2*x5^2 + 10*x2*x6^2 + 10*x2*x7^2 + 10*x2*x8^2 - 11*x2 + 10, 10*x1^2*x3 + 10*x2^2*x3 + 10*x3*x4^2 + 10*x3*x5^2 + 10*x3*x6^2 + 10*x3*x7^2 + 10*x3*x8^2 - 11*x3 + 10, 10*x1^2*x4 + 10*x2^2*x4 + 10*x3^2*x4 + 10*x4*x5^2 + 10*x4*x6^2 + 10*x4*x7^2 + 10*x4*x8^2 - 11*x4 + 10, 10*x1^2*x5 + 10*x2^2*x5 + 10*x3^2*x5 + 10*x4^2*x5 + 10*x5*x6^2 + 10*x5*x7^2 + 10*x5*x8^2 - 11*x5 + 10, 10*x1^2*x6 + 10*x2^2*x6 + 10*x3^2*x6 + 10*x4^2*x6 + 10*x5^2*x6 + 10*x6*x7^2 + 10*x6*x8^2 - 11*x6 + 10, 10*x1^2*x7 + 10*x2^2*x7 + 10*x3^2*x7 + 10*x4^2*x7 + 10*x5^2*x7 + 10*x6^2*x7 + 10*x7*x8^2 - 11*x7 + 10, 10*x1^2*x8 + 10*x2^2*x8 + 10*x3^2*x8 + 10*x4^2*x8 + 10*x5^2*x8 + 10*x6^2*x8 + 10*x7^2*x8 - 11*x8 + 10}):
st := time[real]():
Basis(J, tdeg(x1,x2,x3,x4,x5,x6,x7,x8), characteristic=2^31-1):
time[real]() - st;

J := PolynomialIdeal({10*x1*x2^2 + 10*x1*x3^2 + 10*x1*x4^2 + 10*x1*x5^2 + 10*x1*x6^2 + 10*x1*x7^2 + 10*x1*x8^2 + 10*x1*x9^2 - 11*x1 + 10,
10*x1^2*x2 + 10*x2*x3^2 + 10*x2*x4^2 + 10*x2*x5^2 + 10*x2*x6^2 + 10*x2*x7^2 + 10*x2*x8^2 + 10*x2*x9^2 - 11*x2 + 10,
10*x1^2*x3 + 10*x2^2*x3 + 10*x3*x4^2 + 10*x3*x5^2 + 10*x3*x6^2 + 10*x3*x7^2 + 10*x3*x8^2 + 10*x3*x9^2 - 11*x3 + 10,
10*x1^2*x4 + 10*x2^2*x4 + 10*x3^2*x4 + 10*x4*x5^2 + 10*x4*x6^2 + 10*x4*x7^2 + 10*x4*x8^2 + 10*x4*x9^2 - 11*x4 + 10,
10*x1^2*x5 + 10*x2^2*x5 + 10*x3^2*x5 + 10*x4^2*x5 + 10*x5*x6^2 + 10*x5*x7^2 + 10*x5*x8^2 + 10*x5*x9^2 - 11*x5 + 10,
10*x1^2*x6 + 10*x2^2*x6 + 10*x3^2*x6 + 10*x4^2*x6 + 10*x5^2*x6 + 10*x6*x7^2 + 10*x6*x8^2 + 10*x6*x9^2 - 11*x6 + 10,
10*x1^2*x7 + 10*x2^2*x7 + 10*x3^2*x7 + 10*x4^2*x7 + 10*x5^2*x7 + 10*x6^2*x7 + 10*x7*x8^2 + 10*x7*x9^2 - 11*x7 + 10,
10*x1^2*x8 + 10*x2^2*x8 + 10*x3^2*x8 + 10*x4^2*x8 + 10*x5^2*x8 + 10*x6^2*x8 + 10*x7^2*x8 + 10*x8*x9^2 - 11*x8 + 10,
10*x1^2*x9 + 10*x2^2*x9 + 10*x3^2*x9 + 10*x4^2*x9 + 10*x5^2*x9 + 10*x6^2*x9 + 10*x7^2*x9 + 10*x8^2*x9 - 11*x9 + 10}):
st := time[real]():
Basis(J, tdeg(x1,x2,x3,x4,x5,x6,x7,x8,x9), characteristic=2^31-1):
time[real]() - st;


(*
 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_lda *)
(* prefix:
  lda_xc_1d_ehwlrg_params *params;

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

n := rs -> 1/(2*rs):

f := (rs, zeta) -> \
 (params_a_a1 + params_a_a2*n(rs) + params_a_a3*n(rs)^2) * n(rs)^params_a_alpha:


(*
 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 *)

$define gga_x_b88_params
$include "gga_x_b88.mpl"

cab := 0.63:
css := 0.96:

b88_cab := (rs, z, xs0, xs1) ->
  - 0.8 * (1 - z^2)/4 * n_total(rs)
  * b88_zab(cab, f_b88, rs, z, xs0, xs1)^2 * (
    1 - log(1 + b88_zab(cab, f_b88, rs, z, xs0, xs1))
      / b88_zab(cab, f_b88, rs, z, xs0, xs1)
    ):

b88_css := (rs, z, xs, ts) ->
  - 0.01 * (1 + z)/2 * n_spin(rs, z)^(5/3) * 2*ts * Fermi_D(xs, ts)
  * b88_zss(css, f_b88, rs, z, xs)^4 * (
    1 - 2*log(1 + b88_zss(css, f_b88, rs, z, xs)/2)
      / b88_zss(css, f_b88, rs, z, xs)
    ):

f_b88_c := (rs, z, xs0, xs1, ts0, ts1) ->
  + b88_cab(rs,  z, xs0, xs1)
  + b88_css(rs,  z, xs0, ts0)
  + b88_css(rs, -z, xs1, ts1):

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


# Example from Section 5.3
read "../ComputeIdentifiableFunctions.mpl";

#------------------------------------------------------------------------------
# Auxiliary function for counting nonconstant coefficients

CountNonNumeric := proc(p, var)
    local result, c:
    result := 0:
    for c in coeffs(p, var) do
        if not type(c, numeric) then
            result := result + 1:
        end if:
    end do:
    return result:
end proc:

#------------------------------------------------------------------------------

model_original := [
    diff(x1(t), t) = (a * x1(t) - b * x1(t) * x2(t)),
    diff(x2(t), t) = (-c * x2(t) + d * x1(t) * x2(t)),
    diff(f(t), t) = 0,
    y1(t) = (e * x1(t) + f(t) * x2(t)),
    y2(t) = f(t)
]:

printf("We are interested in computing ME-identifiable functions and the bound for ME-identifiablility\n"):
ME_identifiable_functions := MultiExperimentIdentifiableFunctions(model_original, no_bound=true, simplified_generators=true)[3]:
printf("The field of multi-experiment identifiable functions is generated by %a\n", ME_identifiable_functions):

printf("To find a good bound for the number of experiments we consider the following model (as described in Section 5.3)\n"):
model := [
    diff(x1(t), t) = (a * x1(t) - b * x1(t) * x2(t)),
    diff(x2(t), t) = (-c * x2(t) + d * x1(t) * x2(t)),
    y(t) = (e * x1(t) + f * x2(t))
]:
print(model):

printf("First we compute the bound and IO-equations considering f just as a parameter\n"):
result := MultiExperimentIdentifiableFunctions(model, simplified_generators=true):
printf("If we consider a different system with f being an unknown parameter with the same value for different experiments, the bound from the theorem will be %a\n", result[1]):
# In the notation of Section 5.3
io_eq_coeffs := [expand(denom(result[2][1])), op(map(p -> expand(simplify(p * denom(result[2][1]))), result[2]))]:
print(io_eq_coeffs):
ns := sort(map(p -> CountNonNumeric(p, f), io_eq_coeffs)):
n := ns[1]:
printf("The number of non-numeric coefficients with respect to f (in the notation of Section 5.3: n, n1, n2, ...) in the coefficients of the only input-output equation are: %a\n", ns):
printf("Section 5.3 implies that the number of experiments for the case when f is a known parameter different in different experiments is %a\n", max(n + min(ns[2...]), max(ns[2..]))):


read "../IdentifiabilityODE.mpl";

sys := [
diff(v(t), t) = k*y(t) - v(t)*u,
diff(x(t), t) = lm - x(t)*d - x(t)*v(t)*beta,
diff(z(t), t) = c*w(t)*q*y(t) - h*z(t),
diff(w(t), t) = -b*w(t) + c*w(t)*x(t)*y(t) - c*w(t)*q*y(t),
diff(y(t), t) = x(t)*v(t)*beta - a*y(t),
y2(t) = z(t),
y1(t) = w(t)
];
CodeTools[CPUTime](IdentifiabilityODE(sys, GetParameters(sys)));


read "../IdentifiabilityODE.mpl";

sys := [
diff(x3(t), t) = -b2*x3(t) + b2*x2(t),
diff(x1(t), t) = a2*x0(t) - a2*x1(t),
diff(x2(t), t) = (ka*kc*b1*x3(t) - ka*kc*b1*x2(t) + ka*b1*x0(t)*x3(t) - ka*b1*x0(t)*x2(t) - n*kc*x2(t) + kc*b1*x3(t)*x2(t) - kc*b1*x2(t)^2) / (ka*kc + ka*x0(t) + kc*x2(t)),
diff(x0(t), t) = (-ka*n*x0(t) - ka*kc*a1*x0(t) + ka*kc*a1*x1(t) - ka*a1*x0(t)^2 + ka*a1*x0(t)*x1(t) - kc*a1*x0(t)*x2(t) + kc*a1*x1(t)*x2(t)) / (ka*kc + ka*x0(t) + kc*x2(t)),
y1(t) = x0(t)
];
CodeTools[CPUTime](IdentifiabilityODE(sys, GetParameters(sys)));


toy_input := "start-A
    start-b
    A-c
    A-b
    b-d
    A-end
    b-end":

toy2_input:="dc-end
    HN-start
    start-kj
    dc-start
    dc-HN
    LN-dc
    HN-end
    kj-sa
    kj-HN
    kj-dc":

sample_input := "fs-end
    he-DX
    fs-he
    start-DX
    pj-DX
    end-zg
    zg-sl
    zg-pj
    pj-he
    RW-he
    fs-DX
    pj-RW
    zg-RW
    start-pj
    he-WI
    zg-he
    pj-fs
    start-RW":

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

nodes := map(s->{StringTools:-Split(s,"-")[]}, StringTools:-Split(input)):
G := GraphTheory:-Graph( {nodes[]} );
small := select(StringTools:-IsLower,{map(op, nodes)[]}) minus {"end"};

numpaths := proc(a, s2:=FAIL, visited_s2:=false,
                 excluding:={"start"}, path:=[] )
global G, small;
local newex, newvs2;

    if a = "end" then
       if s2<>FAIL and numboccur(path, s2) <> 2 then
          return 0;
       end if;
#       print([path[],"end"]);
       return 1;
    end if;

    local nb := {GraphTheory:-Neighborhood(G, a)[]} minus excluding;
    if a = s2 and visited_s2 = false then
         newex := excluding;
         newvs2 := true;
    elif a in small then
         newex := excluding union {a};
         newvs2 := visited_s2;
    else
         newex := excluding;
         newvs2 := visited_s2;
    end if;

    return add(numpaths(i, s2, newvs2, newex, [path[],a]), i in nb);

 end proc:

answer1 := numpaths("start");
answer2 := answer1 + add(numpaths("start", pp), pp in small minus {"start"});


######################################################################
# CP(N) is the projective space CP^N

`is_element/CP` := (N::posint) -> proc(x,t := constant)
 if not (type(x,[t $ (N+1)])) then
  return false;
 fi;

 if simplify(x) = [0$(N+1)] then
  return false;
 fi;

 return true;
end:

`is_equal/CP` := (N::posint) -> proc(x,y)
 local i,j;
 for i from 1 to N do
  for j from i+1 to N do
   if simplify(x[i] * y[j] - x[j] * y[i]) <> 0 then
    return false;
   fi;
  od;
 od;

 return true;
end:

`is_leq/CP` := NULL;

`random_element/CP` := (N::posint) -> proc() local i; [seq(`random_element/CC`(),i=1..N+1)] end;

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


 func $floorf64toi32 (
  var %i f64
  ) i32 {
   return (
     floor i32 f64(dread f64 %i))}

 func $floorf64toi64 (
  var %i f64
  ) i64 {
   return (
     floor i64 f64(dread f64 %i))}

 func $floorf32toi32 (
  var %i f32
  ) i32 {
   return (
     floor i32 f32(dread f32 %i))}

 func $floorf32toi64 (
  var %i f32
  ) i64 {
   return (
     floor i64 f32(dread f32 %i))}

# todo float floor
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


read "../IdentifiabilityODE.mpl";

sys := [
diff(S(t), t) = -bi*S(t)*II(t) - S(t)*mu - S(t)*bw*W(t) + R(t)*a + mu,
diff(II(t), t) = bi*S(t)*II(t) - gam*II(t) + S(t)*bw*W(t) - mu*II(t),
diff(W(t), t) = xi*II(t) - xi*W(t),
diff(R(t), t) = gam*II(t) - R(t)*mu - R(t)*a,
y(t) = k*II(t),
y2(t) = S(t) + R(t) + II(t)
];
CodeTools[CPUTime](IdentifiabilityODE(sys, GetParameters(sys)));


#
#     Test Suite for Reparam---integration and summation changes of variables
#
#Created by Carl 17Jun2016.

#common header for Hakaru-Maple test suites
kernelopts(assertlevel= 2): # be strict on all assertions while testing
kernelopts(opaquemodules= false): # allow testing of internal routines
if not NewSLO :: `module` then
  WARNING("loading NewSLO failed");
  `quit`(3)
end if:

with(Hakaru):
with(KB):
with(NewSLO):
########################

#common procedures for these tests.

TestReparam:= proc(
     e_in,
     e_out,   #Expected output, after postprocessing by `verify` and `&under`
     #`&under` is defined below.
     #Type `verification` is slightly modified (below) from its standard definition.
     ver::verification,
     #First infolevel is for initial test; second is for retest after failure.
     {infolevels::[nonnegative, nonnegative]:= [0, Float(infinity)]},
     {ctx::list:= []}   #Assumptions (not necessarily to be passed to `assuming`)
)
# For 'production' runs, we want things to be as quiet
# as possible.  We would like to do
# maple -q < ReparamT.mpl | grep -v "passed"
# and get 0 lines of output. If global `pretty`	is true, some extra lines are
# printed for readability. You may want to set this in a worksheet session.
local
     LOform:= toLO(e_in),  #preprocess input
     r
;
     if :-pretty then printf("\n") end if;
     :-infolevel[reparam]:= infolevels[1];
     if not CodeTools:-Test(reparam(LOform, :-ctx= ctx), e_out, ver, boolout, _rest) then
          #If test fails, do same test with diagnostic infolevel setting.
          if infolevels[2] > infolevels[1] then
               printf("\n@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"  );
               printf("\n*** Rerun at higher infolevel: ***\n");
               :-infolevel[reparam]:= infolevels[2];
               r:= CodeTools:-Test(reparam(LOform, :-ctx= ctx), e_out, ver, boolout, _rest);
               printf(  "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\n");
               return r
          else
               return false
          end if
     else
          return
     end if
end proc:

#VerifyTools is an undocumented Maple library package similiar to TypeTools.
#Add a verification &under analogous to type &under.
VerifyTools:-AddVerification(`&under`= ((e1,e2,Ver,f)-> verify(f(e1,_rest), e2, Ver))):

#Need to overload Maple library `type/verification` so that expressions of the form 'v &under f'
#are considered verifications.
`type/verification/MapleLibrary`:= eval(`type/verification`):
`type/verification`:= overload([
     proc(Under::specfunc(`&under`))
     option overload;
          type(op(1,Under), `verification/MapleLibrary`)
     end proc,

     `type/verification/MapleLibrary`
]):

#Automatic simplifications: When an easy transformation can be used to
#transform a distribution that we don't support to one that we do, then a
#procedure in this section does it. These may be moved to NewSLO.

#Bernoulli
Bern:= proc(p)
     local i;
     Bind(Counting(0,1), i, Weight(idx([p,1-p],i), Ret(i)))
end proc:

#Exponential
Exponential:= mu-> GammaD(1,mu):

#Control printing of extra lines for readability.
if not assigned(pretty) then pretty:= false end if:

#
#   The tests
#

#Constant multiple in integral
TestReparam(
     Bind(Lebesgue(-infinity,infinity), x, Weight(2, Ret(2*x))),
     Lebesgue(-infinity,infinity),
     equal &under fromLO,
     label= "(t0a) Constant multiple with Lebesgue" #passing
):

#Logarithm with Uniform
TestReparam(
     Bind(Uniform(0,1), x, Ret(-log(x))),
     GammaD(1,1),
     equal &under fromLO,
     label= "(t0b) Logarithm with Uniform" #passing
):

#(t1) Symbolic affine transformation with Gaussian
TestReparam(
     Bind(Gaussian(mu,sigma), x, Ret((x-mu)/sigma)),
     Gaussian(0,1),
     equal &under fromLO,
     ctx= [sigma > 0],
     label= "(t1) Symbolic affine transformation with Gaussian" #passing
):

#(t2) ChiSquared to Gamma
#(inverse of relationship given in Relationships.hs)
# Special case of Gamma now recognized as being ChiSquared
TestReparam(
     ChiSquared(2*b),
     GammaD(b,2),
     equal &under (fromLO, _ctx= foldr(assert, empty, b > 0)),
     ctx= [b > 0],
     label= "(t2) ChiSquared to Gamma" #passing
):

#(t3) Symbolic constant multiple with Gamma
TestReparam(
     Bind(GammaD(alpha,beta), x, Ret(2*x/beta)),
     GammaD(alpha, 2),
     equal &under (fromLO, _ctx= foldr(assert, empty, alpha > 0, beta > 0)),
     ctx= [alpha > 0, beta > 0],
     label= "(t3) Symbolic constant multiple with Gamma" #passing
):

#(t4) Two-variable LFT with Gamma
#This fails due to multivariate changes of variable not being implemented yet.
TestReparam(
     Bind(GammaD(a,t), x1, Bind(GammaD(b,t), x2, Ret(x1/(x1+x2)))),
     BetaD(a,b),
     equal &under fromLO,
     infolevels= [2,2],
     label= "(t4) Two-variable LFT with Gamma (currently failing)"
):

#(t5) Logarithm with symbolic constant multiplier and Uniform
TestReparam(
     Bind(Uniform(0,1), x, Ret(-alpha*log(x))),
     GammaD(1,alpha),
     equal &under fromLO,
     ctx= [alpha >= 0],
     label= "(t5) Logarithm with symbolic constant multiplier and Uniform"
     #passing
):

#(t6) Poisson(mu) -> ? as mu -> infinity
#The concept of taking the limit at infinity of a distribution parameter isn't
#implemented yet.

#(t7) Quotient of standard normals to standard Cauchy
#Fails because it's multivariate.
#This should be the first multivariate integration to implement, due to
#simplicity; it might work as a simple multivariate substitution.
TestReparam(
     Bind(Gaussian(0,1), x1, Bind(Gaussian(0,1), x2, Ret(x1/x2))),
     Cauchy(0,1),
     equal &under fromLO,
     infolevels= [2,2],
     label=
          "(t7) Quotient of standard normals to standard Cauchy "
          "(currently failing)"
):

#(t8) Affine translation of quotient of standard normals to Cauchy
#This is a trivial variation of t7.
#Fails because it's multivariate.
TestReparam(
     Bind(Gaussian(0,1), x1, Bind(Gaussian(0,1), x2, Ret(a+alpha*x1/x2))),
     Cauchy(a,alpha),
     equal &under (fromLO, _ctx= foldr(assert, empty, alpha > 0)),
     ctx= [alpha > 0],
     infolevels= [2,2],
     label=
          "(t8) Affine translation of quotient of standard normals to Cauchy "
          "(currently failing)"
):

#(t9) Bernoulli to Binomial with n=1
#Fails because Binomial isn't implemented.
#Doesn't use reparam in a meaningful way.
TestReparam(
     Bern(p),
     Binomial(1,p),
     equal &under (fromLO, _ctx= foldr(assert, empty, 0 <= p, p <= 1)),
     ctx= [0 <= p, p <= 1],
     infolevels= [2,2],
     label= "(t9) Bernoulli to Binomial with n=1 (currently failing)"
):

#(t10) Beta(1,1) to Uniform(0,1)
#This one doesn't require reparam, but it passes using reparam.
TestReparam(
     #Note: No use of `Bind`. This passes by direct recognition by fromLO
     #without any meaningful use of reparam, although reparam is called.
     BetaD(1,1),
     Uniform(0,1),
     equal &under fromLO,
     label= "(t10) Beta(1,1) to Uniform(0,1)" #passing
):

#(t11) Laplace to difference of Gammas
#This one fails because Laplace isn't implemented. Also, there's nothing that
#reparam can do with this. It's up to fromLO to recognize the compound
#expression.
#Perhaps this should be an automatic simplification.
TestReparam(
     Laplace(a1,a2),
     Bind(GammaD(1,a1), x1, Bind(GammaD(1,a2), x2, Ret(x1-x2))),
     equal &under (fromLO, _ctx= foldr(assert, empty, a1 > 0, a2 > 0)),
     ctx= [a1 > 0, a2 > 0],
     infolevels= [2,2],
     label= "(t11) Laplace to difference of Gammas (currently failing)"
):

#(t12) Sum of iid Exponentials to Gamma
#This currently fails because it's multivariate.
TestReparam(
     Bind(Exponential(b), x1, Bind(Exponential(b), x2, Ret(x1+x2))),
     GammaD(2,b),
     equal &under (fromLO, _ctx= foldr(assert, empty, b > 0)),
     ctx= [b > 0],
     infolevels= [2,2],
     label= "(t12) Sum of iid Exponentials to Gamma (currently failing)"
):

#(t13) Weibull(1,b) to Exponential(1/b)
#This test is wrong in Relationships.hs!
#It should be Weibull(a,1) to Exponential(1/a).
#Fails because Weibull isn't implemented.
TestReparam(
     #Straightforward recognition; doesn't use reparam in a meaningful way.
     Weibull(a,1),
     Exponential(1/a),
     equal &under (fromLO, _ctx= foldr(assert, empty, a > 0)),
     ctx= [a > 0],
     infolevels= [2,2],
     label= "(t13) Weibull(a,1) to Exponential(1/a) (currently failing)"
):

#(t14) Standard normal over sqrt(ChiSquared) to StudentT
#Fails because it's multivariate.
TestReparam(
     Bind(Gaussian(0,1), x, Bind(ChiSquared(nu), y, Ret(x/sqrt(y/nu)))),
     StudentT(nu, 0, 1),
     equal &under (fromLO, _ctx= foldr(assert, empty, nu > 0)),
     ctx= [nu > 0],
     infolevels= [2,2],
     label= "(t14) Std normal over sqrt(Chi2) to StudentT (currently failing)"
):

#(t15) 1/InverseGamma(k,1/t) to GammaD(k,t)
TestReparam(
     Bind(InverseGammaD(k, 1/t), x, Ret(1/x)),
     GammaD(k,t),
     equal &under (fromLO, _ctx= foldr(assert, empty, k > 0, t > 0)),
     ctx= [k > 0, t > 0],
     label= "(t15) 1/InverseGammaD(k,1/t) to GammaD(k,t)" #passing
):

#(t16) Sum of std normals to normal(0,sqrt(2))
#Fails because it requires a multivariate change of vars.
TestReparam(
     Bind(Gaussian(0,1), x1, Bind(Gaussian(0,1), x2, Ret(x1+x2))),
     Gaussian(0,sqrt(2)),
     equal &under fromLO,
     infolevels= [2,2],
     label= "(t16) Sum of std normals to normal(0,sqrt(2)) (currently failing)"
):

#(t17) Sum of std normal and normal to normal
#Fails because it requires a multivariate change of vars.
TestReparam(
     Bind(Gaussian(0,1), x1, Bind(Gaussian(mu,sigma), x2, Ret(x1+x2))),
     Gaussian(mu, sqrt(1+sigma^2)),
     equal &under (fromLO, _ctx= foldr(assert, empty, sigma > 0)),
     ctx= [sigma > 0],
     infolevels= [2,2],
     label= "(t17) Sum of std normal and normal to normal (currently failing)"
):

#(t18) Sum of symbolic multiples of std normals to normal
#Fails because it requires a multivariate change of vars.
TestReparam(
     Bind(Gaussian(0,1), x1, Bind(Gaussian(0,1), x2, Ret(a1*x1 + a2*x2))),
     Gaussian(0, sqrt(a1^2 + a2^2)),
     equal &under fromLO,
     infolevels= [2,2],
     label=
          "(t18) Sum of symbolic multiples of std normals to normal "
          "(currently failing)"
):

#(t19) Sum of equiprobable binomials to binomial
#Fails for three reasons: it's multivariate, it's discrete, and Binomial isn't
#implemented.
TestReparam(
     Bind(Binomial(n1,p), x1, Bind(Binomial(n2,p), x2, Ret(x1+x2))),
     Binomial(n1+n2, p),
     equal &under (fromLO,
          _ctx= foldr(
               assert, empty,
               n1::integer, n1 > 0, n2::integer, n2 > 0, 0 <= p, p <= 1
          )
     ),
     ctx= [n1::integer, n1 > 0, n2::integer, n2 > 0, 0 <= p, p <= 1],
     infolevels= [2,2],
     label=
          "(t19) Sum of equiprobable binomials to binomial "
          "(currently failing)"
):

#(t20) Sum of n Bernoullis to Binomial
#Fails because Binomial isn't implemented.
TestReparam(
     Bind(Plate(n, i, Bern(p)), xs, Ret(sum(idx(xs,i),i=0..n-1))),
     Binomial(n,p),
     equal &under (fromLO,
          _ctx= foldr(assert, empty, n::integer, n > 0, 0 <= p, p <= 1)
     ),
     ctx= [n::integer, n > 0, 0 <= p, p <= 1],
     infolevels= [2,2],
     label= "(t20) Sum of n Bernoullis to Binomial (currently failing)"
):

#(t21) Sum of Poissons to Poisson
#Fails because reparam doesn't yet handle summations.
TestReparam(
     Bind(PoissonD(lambda1), x1, Bind(PoissonD(lambda2), x2, Ret(x1+x2))),
     PoissonD(lambda1 + lambda2),
     equal &under (fromLO,
          _ctx= foldr(assert, empty, lambda1 > 0, lambda2 > 0)
     ),
     ctx= [lambda1 > 0, lambda2 > 0],
     infolevels= [2,2],
     label= "(t21) Sum of Poissons to Poisson (currently failing)"
):

#(t22) Sum of Gammas to Gamma
#Fails because it's multivariate.
TestReparam(
     Bind(GammaD(a1,b), x1, Bind(GammaD(a2,b), x2, Ret(x1+x2))),
     GammaD(a1+a2, b),
     equal &under (fromLO, _ctx= foldr(assert, empty, a1 > 0, a2 > 0, b > 0)),
     ctx= [a1 > 0, a2 > 0, a3 > 0],
     infolevels= [2,2],
     label= "(t22) Sum of Gammas to Gamma (currently failing)"
):

#(t23) Sum of n Exponentials to Gamma
#Fails because it involves a Plate.
TestReparam(
     Bind(Plate(n, i, Exponential(b)), xs, Ret(sum(idx(xs,i), i= 0..n-1))),
     GammaD(n,b),
     equal &under (fromLO, _ctx= foldr(assert, empty, n::integer, n > 0, b > 0)),
     ctx= [n::integer, n > 0, b > 0],
     infolevels= [2,2],
     label= "(t23) Sum of n Exponentials to Gamma (currently failing)"
):

#(t24) Weibull "scaling" property.
#I can find no evidence that this is true; indeed, it seems trivial to disprove
#it.

#(t25) Product of lognormals is lognormal
#Fails because it's multivariate.
TestReparam(
     Bind(
          Gaussian(mu1,sigma1), x1,
          Bind(Gaussian(mu2,sigma2), x2, Ret(exp(x1)*exp(x2)))
     ),
     Bind(Gaussian(mu1+mu2, sqrt(sigma1^2 + sigma2^2)), x3, Ret(exp(x3))),
     equal &under (fromLO, _ctx= foldr(assert, empty, sigma1 > 0, sigma2 > 0)),
     ctx= [sigma1 > 0, sigma2 > 0],
     infolevels= [2,2],
     label= "(t25) Product of lognormals is lognormal (currently failing)"
):

#(t26) Cauchy of reciprocal to Cauchy
#Fails due to improper handling of the discontinuity in the substitution.
TestReparam(
     Bind(Cauchy(0, sigma), x, Ret(1/x)),
     Cauchy(0, 1/sigma),
     equal &under (fromLO, _ctx= foldr(assert, empty, sigma > 0)),
     ctx= [sigma > 0],
     infolevels= [2,2],
     label= "(t26) Cauchy of reciprocal to Cauchy (currently failing)"
):

#(t27) Symbolic constant multiple of Gamma to Gamma
TestReparam(
     Bind(GammaD(r,lambda), x, Ret(a*x)),
     GammaD(r, a*lambda),
     equal &under (fromLO,
          _ctx= foldr(assert, empty, r > 0, lambda > 0, a > 0)
     ),
     ctx= [r > 0, lambda > 0, a > 0],
     label= "(t27) Symbolic constant multiple of Gamma to Gamma" #passing
):

#(t28) Beta of 1-x to Beta
TestReparam(
     Bind(BetaD(a,b), x, Ret(1-x)),
     BetaD(b,a),
     equal &under (fromLO, _ctx= foldr(assert, empty, a > 0, b > 0)),
     ctx= [a > 0, b > 0],
     label= "(t28) Beta with 1-x to Beta" #passing
):

#(t29) Binomial of n-x to Binomial
#Fails because Binomial isn't implemented.
TestReparam(
     Bind(Binomial(n,p), x, Ret(n-x)),
     Binomial(n, 1-p),
     equal &under (fromLO,
          _ctx= foldr(assert, empty, n::integer, n > 0, 0 <= p, p <= 1)
     ),
     ctx= [n::integer, n > 0, 0 <= p, p <= 1],
     infolevels= [2,2],
     label= "(t29) Binomial of n-x to Binomial (currently failing)"
):

#(t30) Square of std normal to ChiSquared(1)
#Fails due to non-invertibility of substitution u= x^2.
TestReparam(
     Bind(Gaussian(0,1), x, Ret(x^2)),
     ChiSquared(1),
     equal &under fromLO,
     infolevels= [2,2],
     label= "(t30) Square of std normal to ChiSquared(1) (currently failing)"
):

#(t31) Sum of squares of n iid std normals to ChiSquared(n)
#Fails because it involves a Plate.
TestReparam(
     Bind(Plate(n, i, Gaussian(0,1)), xs, Ret(sum(idx(xs,i)^2, i= 0..n-1))),
     ChiSquared(n),
     equal &under (fromLO, _ctx= foldr(assert, empty, n::integer, n > 0)),
     ctx= [n::integer, n > 0],
     infolevels= [2,2],
     label=
          "(t31) Sum of squares of n iid std normals to ChiSquared(n) "
          "(currently failing)"
):

#(t32) Sum of ChiSquareds is ChiSquared
#Fails because it's multivariate.
TestReparam(
     Bind(ChiSquared(k1), x1, Bind(ChiSquared(k2), x2, Ret(x1+x2))),
     ChiSquared(k1+k2),
     equal &under (fromLO, _ctx= foldr(assert, empty, k1 > 0, k2 > 0)),
     ctx= [k1 > 0, k2 > 0],
     infolevels= [2,2],
     label= "(t32) Sum of ChiSquareds is ChiSquared (currently failing)"
):