######################################################################
# 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;


# 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)));


# 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"});


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)));


# Kinematik-Berechnung Modell PalH2m1 (Hybrider Palettierer, Variante 2, Modellierung 1)
# Beschreibung
# Ein Palettierroboter mit fünf Achsen als Näherung für einen Palettierroboter mit Viergelenkketten
# Die vierte Achse des Ersatzmodells wird als abhängiges Gelenk in Abhängigkeit der Achsen 2 und 3 beschrieben.
# Dadurch wird der Effekt der Parallelogramme kinematisch nachgebildet.
# Die Gelenkmomente sollten genauso wie durch die Parallelogramme übertragen werden (als ob deren Dynamikparameter Null sind).
# Quellen
# Autor
# Moritz Schappler, moritz.schappler@imes.uni-hannover.de, 2018-12
# (C) Institut für Mechatronische Systeme, Universität Hannover
# Initialisierung
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
kin_constraints_exist := true: # Für Speicherung
;
with(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_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_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
# Variable mit Winkeln der Zwangsbedingungen nur in Abhängigkeit der verallgemeinerten Koordinaten
kintmp_qs := Matrix(RowDimension(kintmp_s),1):
kintmp_qt := Matrix(RowDimension(kintmp_s),1):
# Ersetzungsausdrücke definieren.
# Variable zum speichern des Sinus und Cosinus der Winkel. Für dieses System ist das eigentlich nicht notwendig. Erstelle Variable, da sie von den anderen Skripten erwartet wird
kintmp_subsexp := Matrix(2*RowDimension(kintmp_s),2):
# Zwangsbedingung für abhängigen Gelenkwinkel
# Der vierte Winkel ist der passive Winkel nahe des Endeffektors. Er wird durch die Parallelstruktur (aus zwei Parallelogrammen) bei hybriden Palettierrobotern bestimmt.
# Durch die Parallelogramme ergibt sich ein konstantes Verhältnis zu den Gelenkwinkeln der Achsen 2 und 3, die auch in derselben Ebene liegen.
rho4_qt := -theta(2) -theta(3):
kintmp_qt(1,1) := rho4_qt:
kintmp_qs := convert_t_s(kintmp_qt):
# Exportiere Code für folgende Skripte
# Speichere Maple-Ausdruck (Eingabe-Format und internes Format)
save kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_subsexp_maple", robot_name):
save kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_subsexp_maple.m", robot_name):
printf("Ausdrücke für kintmp_subsexp gespeichert (Maple)\n"):
save kintmp_qs, kintmp_qt, kin_constraints_exist, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert", robot_name):
save kintmp_qs, kintmp_qt, kin_constraints_exist, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
save kintmp_qs, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_qs_maple_inert", robot_name):
# Liste mit abhängigen konstanten Kinematikparametern erstellen (wichtig für Matlab-Funktionsgenerierung)
read "../helper/proc_list_constant_expressions";
kc_symbols := Matrix(list_constant_expressions( kintmp_qs )):
save kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_maple", robot_name):
MatlabExport(Transpose(kc_symbols), sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_matlab.m", robot_name), 2):
printf("Fertig\n"):


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

n_mbeef := 5:
coefs_mbeef := [
    [ 1.17114923e+00,  1.15594371e-01, -5.32167416e-02, -2.01131648e-02,  1.41417107e-03],
    [-6.76157938e-02,  4.53837246e-02, -2.22650139e-02,  1.92374554e-02,  9.19317034e-07],
    [ 1.48659502e-02,  3.18024096e-02, -5.21818079e-03,  1.33707403e-07, -5.00749348e-07],
    [ 1.40794142e-03, -6.08338264e-03, -6.57949254e-07, -5.49909413e-08,  5.74317889e-08],
    [ 1.41530486e-04, -1.00478906e-07,  2.01895739e-07,  3.97324768e-09, -3.40722258e-09]
]:

$include "mbeef.mpl"

f := (rs, x, t, u) ->
  mgga_mbeef(x, t):


(*
 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_gga_x *)
(* prefix:
  gga_x_mpbe_params *params;

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

f0 := s -> s^2/(1 + params_a_a*s^2):
f := x -> 1
  + params_a_c1*f0(X2S*x)
  + params_a_c2*f0(X2S*x)^2
  + params_a_c3*f0(X2S*x)^3:


(*
 Copyright (C) 2017 M.A.L. Marques
               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: mgga_exc *)
(* prefix:
  mgga_x_gdme_params *params;

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

gdme_at := (params_a_AA + 3/5*params_a_BB)*2^(1/3)/(X_FACTOR_C*(3*Pi^2)^(2/3)):
gdme_bt := params_a_BB/(X_FACTOR_C*2^(1/3)*(3*Pi^2)^(4/3)):

gdme_f := (x, u, t) -> gdme_at + gdme_bt*((params_a_a^2 - params_a_a + 1/2)*u - 2*t):

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


######################################
# Implementation of the lapse choices
# in standard G-BSSN
# Gamma drive and 1 + log
# Also some experimental other lapse
# choices
######################################

read "/home/arman/FD/FD.mpl":

CFD();
MFD();

grid_functions:= {alpha,K,beta,Lamx,DLamx,BB,ctfmp};

# A generalization of 1 + log
eq1 := diff(alpha(t,x),t) = -gma*alpha(t,x)^gmb*K(t,x);

AVGT := f -> (FD(f,[[1],[0,0,0]]) + FD(f,[[0],[0,0,0]]))/2 ;
FD_table[t] := [ [0] , [0,1] ];

spec := [
  { i = [1,1,1] , b=xmin } = Gen_Sten(lhs(eq1)) - AVGT(Gen_Sten(rhs(eq1))) ,
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq1)) - AVGT(Gen_Sten(rhs(eq1))),
  { i=[Nx,Nx,1], b=xmax  } = alpha(n+1,i) + myzero*x(i) - 1
];

A_Gen_Solve_Code(spec,{alpha(n+1,i)},proc_name="evol_alphaopl");
A_Gen_Res_Code(spec,input="d",proc_name="resid_alphaopl");

# Some kinda generalization of 1 + log
eq2:= diff(alpha(t,x),t)  + epsal*diff(K(t,x),t) =  -epsal*ck*K(t,x);

spec2 := [
  { i = [1,1,1] , b=xmin } = Gen_Sten(lhs(eq2)) - AVGT(Gen_Sten(rhs(eq2))) ,
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq2)) - AVGT(Gen_Sten(rhs(eq2))),
  { i=[Nx,Nx,1], b=xmax  } = alpha(n+1,i) + myzero*x(i) - 1
];

A_Gen_Solve_Code(spec2,{alpha(n+1,i)},proc_name="evol_alphakd");
A_Gen_Res_Code(spec2,input="d",proc_name="resid_alphakd");

# This is gamma drive

eq3 := diff(beta(t,x),t) = mus*Lamx(t,x) - eta*beta(t,x);
eq4 := diff(alpha(t,x),t) = -2*alpha(t,x)*K(t,x);

spec := [
  { i = [1,1,1] , b=xmin } = Gen_Sten(lhs(eq4)) - AVGT(Gen_Sten(rhs(eq4))) ,
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq4)) - AVGT(Gen_Sten(rhs(eq4))),
  { i=[Nx,Nx,1], b=xmax  } = alpha(n+1,i) + myzero*x(i) - 1
];

A_Gen_Solve_Code(spec,{alpha(n+1,i)},proc_name="evol_alphadyopl");
A_Gen_Res_Code(spec,input="d",proc_name="resid_alphadyopl");

spec := [
  { i = [1,1,1] , b=xmin } = Gen_Sten(lhs(eq3)) - AVGT(Gen_Sten(rhs(eq3))) ,
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq3)) - AVGT(Gen_Sten(rhs(eq3))),
  { i=[Nx,Nx,1], b=xmax  } = beta(n+1,i) + myzero*x(i)
];

A_Gen_Solve_Code(spec,{beta(n+1,i)},proc_name="evol_betagd");
A_Gen_Res_Code(spec,input="d",proc_name="resid_betagd");

# Some strange driver, for testing
eq5 := diff(beta(t,x),t) = mus*diff(K(t,x),x) - eta*beta(t,x);

spec := [
  { i = [1,1,1] , b=xmin } = beta(n+1,i) + myzero*x(i) ,
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq5)) - AVGT(Gen_Sten(rhs(eq5))),
  { i=[Nx,Nx,1], b=xmax  } = beta(n+1,i) + myzero*x(i)
];

A_Gen_Solve_Code(spec,{beta(n+1,i)},proc_name="evol_betakd");
A_Gen_Res_Code(spec,input="d",proc_name="resid_betakd");

# More playing around with shift condition
eq6:= diff(beta(t,x),t)  + epsal*diff(Lamx(t,x),t) =  -epsal*ck*Lamx(t,x);

spec2 := [
  { i = [1,1,1] , b=xmin } = Gen_Sten(lhs(eq6)) - AVGT(Gen_Sten(rhs(eq6))) ,
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq6)) - AVGT(Gen_Sten(rhs(eq6))),
  { i=[Nx,Nx,1], b=xmax  } = beta(n+1,i) + myzero*x(i)
];

A_Gen_Solve_Code(spec2,{beta(n+1,i)},proc_name="evol_betagd2");
A_Gen_Res_Code(spec2,input="d",proc_name="resid_betagd2");

eq7 := diff(beta(t,x),t) = advc*beta(t,x)*diff(beta(t,x),x)/ctfmp(x) + mus*BB(t,x);
eq8 := diff(BB(t,x),t) = advc*beta(t,x)*diff(BB(t,x),x)/ctfmp(x) - eta*BB(t,x) + DLamx(t,x);
eq9 := diff(alpha(t,x),t) = advc*beta(t,x)*diff(alpha(t,x),x)/ctfmp(x) - 2*alpha(t,x)*K(t,x);

spec := [
  { i = [1,1,1] , b=xmin } = beta(n+1,i)+myzero*x(i) ,
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq7)) - AVGT(Gen_Sten(rhs(eq7))),
  { i=[Nx,Nx,1], b=xmax  } = beta(n+1,i) + myzero*x(i)
];

A_Gen_Solve_Code(spec,{beta(n+1,i)},proc_name="evol_beta_hgd_adv");
A_Gen_Res_Code(spec,input="d",proc_name="resid_beta_hgd_adv");

spec := [
  { i = [1,1,1] , b=xmin } = BB(n+1,i)+myzero*x(i) ,
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq8)) - AVGT(Gen_Sten(rhs(eq8))),
  { i=[Nx,Nx,1], b=xmax  } = BB(n+1,i) + myzero*x(i)
];

A_Gen_Solve_Code(spec,{BB(n+1,i)},proc_name="evol_BB_hgd_adv");
A_Gen_Res_Code(spec,input="d",proc_name="resid_BB_hgd_adv");

spec := [
  { i = [1,1,1] , b=xmin } =  Gen_Sten(lhs(eq9)) - AVGT(Gen_Sten(rhs(eq9))),
  { i = [2,Nx-1,1]       } = Gen_Sten(lhs(eq9)) - AVGT(Gen_Sten(rhs(eq9))),
  { i=[Nx,Nx,1], b=xmax  } = alpha(n+1,i) - 1 + myzero*x(i)
];

A_Gen_Solve_Code(spec,{alpha(n+1,i)},proc_name="evol_alpha_hgd_adv");
A_Gen_Res_Code(spec,input="d",proc_name="resid_alpha_hgd_adv");


######################################################
# Performing the compactificatoin and non-uniform
# coordinate transformation on the tensor expressions
######################################################

all_pdes:=[
   [DPH,0,dn] , [dg,2,dn] ,
         [DDL,2,dn],
   [DTK,0,dn],
         [RR,2,dn],  [P,2,dn],
   [DLAM,1,up],
       [C1,0,dn],
   [HS,0,dn],
   [DB,0,dn],
   [HS3,0,dn],
   [MK,1,up],
   [EQRPIb2,0,dn],
   [EQIPIb2,0,dn],
   [DRPSI,0,dn],
   [DIPSI,0,dn]
             ];

# Going from x -> ctfm(x) using DEtools
with(DEtools);
addcoords(compact,[t,x],[t,ctfm(x)]);

# See built_eqns.mpl for how 'tt' table is defined

for ii from 1 to nops(all_pdes) do
   nm := all_pdes[ii][1]:
   rank := all_pdes[ii][2]:
   tp := all_pdes[ii][3]:
   printf("Transforming %a\n",nm);
   if rank <> 0 then

    for jj from 1 to nops(coord) do
     if (tt[nm][all][coord[jj]] <> 0 ) then
      tt[nm][allcomp][coord[jj]] := PDEchangecoords(tt[nm][all][coord[jj]],[t,x],compact,[t,x]);
      tt[nm][regcomp][coord[jj]] := PDEchangecoords(tt[nm][reg][coord[jj]],[t,x],compact,[t,x]);

	   # Symbolically replacing derivatives of compactification function
      # since we do not want to finite difference them)

      tt[nm][allcomp][coord[jj]] := subs({diff(ctfm(x),x)=ctfmp(x),diff(ctfm(x),x,x)=ctfmpp(x)},tt[nm][allcomp][coord[jj]]);
      tt[nm][regcomp][coord[jj]] := subs({diff(ctfm(x),x)=ctfmp(x),diff(ctfm(x),x,x)=ctfmpp(x)},tt[nm][regcomp][coord[jj]]);

      # DEtools will replace x with ctfm(x) symbolically in our expressions, but ctfm(x) is our new x, fixing that
      for kk from 1 to nops(grid_functions) do
        fn := grid_functions[kk];
        tt[nm][allcomp][coord[jj]] := subs(fn(t,ctfm(x))=fn(t,x),tt[nm][allcomp][coord[jj]]);
        tt[nm][regcomp][coord[jj]] := subs(fn(t,ctfm(x))=fn(t,x),tt[nm][regcomp][coord[jj]]);
      end do;

     end if;

    end do;
   else

     # Same as above, symbolically replacing derivatives of compactification function
     if ( tt[nm][all] <> 0 ) then
      tt[nm][allcomp] := PDEchangecoords(tt[nm][all],[t,x],compact,[t,x]);
      tt[nm][regcomp] := PDEchangecoords(tt[nm][reg],[t,x],compact,[t,x]);
      tt[nm][allcomp] := subs({diff(ctfm(x),x)=ctfmp(x),diff(ctfm(x),x,x)=ctfmpp(x)},tt[nm][allcomp]);
      tt[nm][regcomp] := subs({diff(ctfm(x),x)=ctfmp(x),diff(ctfm(x),x,x)=ctfmpp(x)},tt[nm][regcomp]);

      # Same as above, fixing DEtools mess!
      for kk from 1 to nops(grid_functions) do
       fn := grid_functions[kk];
       tt[nm][allcomp] := subs(fn(t,ctfm(x))=fn(t,x),tt[nm][allcomp]);
       tt[nm][regcomp] := subs(fn(t,ctfm(x))=fn(t,x),tt[nm][regcomp]);
      end do;

     end if;

   end if;
end do:

# Checking if everything is transformed properly at the limit ctfm(x) = x

for ii from 1 to nops(all_pdes) do
   nm := all_pdes[ii][1]:
   rank := all_pdes[ii][2]:
   tp := all_pdes[ii][3]:
   if rank <> 0 then

			 for jj from 1 to nops(coord) do
            if ( tt[nm][all][coord[jj]] <> 0 ) then
				res := simplify(tt[nm][all][coord[jj]] - eval(tt[nm][allcomp][coord[jj]],{ctfm(x) = x, ctfmp(x) = 1, ctfmpp(x) = 0}));
				printf("res = %a\n",res);
				res := simplify(tt[nm][reg][coord[jj]] - eval(tt[nm][regcomp][coord[jj]],{ctfm(x) = x, ctfmp(x) = 1, ctfmpp(x) = 0}));
				printf("res = %a\n",res);
            end if;
			 end do;

   else
      res := simplify(tt[nm][all] - eval(tt[nm][allcomp],{ctfm(x) = x, ctfmp(x) = 1, ctfmpp(x) = 0}));
      printf("res = %a\n",res);
      res := simplify(tt[nm][reg] - eval(tt[nm][regcomp],{ctfm(x) = x, ctfmp(x) = 1, ctfmpp(x) = 0}));
      printf("res = %a\n",res);
   end if;
end do:

##################################################
# Performing a collection operation, i.e.
# powers of ctfm(x) function, this will help the
# intel c and fortran compiler a lot!
#################################################

for ii from 1 to nops(all_pdes) do
   nm := all_pdes[ii][1]:
   rank := all_pdes[ii][2]:
   tp := all_pdes[ii][3]:
   if rank <> 0 then

			 for jj from 1 to nops(coord) do
            if (tt[nm][all][coord[jj]] <> 0 ) then
  			   tt[nm][all][coord[jj]] := collect(collect(eval(expand(tt[nm][allcomp][coord[jj]]),{ctfmpp(x)=octfmp(x)*ctfmp(x)^2}),1/ctfmp(x)),1/ctfm(x));
			   tt[nm][reg][coord[jj]] := collect(collect(eval(expand(tt[nm][regcomp][coord[jj]]),{ctfmpp(x)=octfmp(x)*ctfmp(x)^2}),1/ctfmp(x)),1/ctfm(x));
				print(tt[nm][all][coord[jj]]);
            print(tt[nm][reg][coord[jj]]);
            end if;
			 end do;

   else
      tt[nm][all] :=  collect(collect(eval(expand(tt[nm][allcomp]),{ctfmpp(x)=octfmp(x)*ctfmp(x)^2}),1/ctfmp(x)),1/ctfm(x));
      tt[nm][reg] :=  collect(collect(eval(expand(tt[nm][regcomp]),{ctfmpp(x)=octfmp(x)*ctfmp(x)^2}),1/ctfmp(x)),1/ctfm(x));
	   print(tt[nm][all]);
	   print(tt[nm][reg]);
   end if;
end do:

# Adding new members of grid functions to our set:
grid_functions := grid_functions union {ctfm,ctfmp,octfmp};