# Base Parameter Regresor Inverse Dynamics for Robot based on MDH frames
# Einleitung
# Berechnung der inversen Dynamik in Regressorform
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# chain -> Berechnung für eine serielle Struktur (nicht: Baumstruktur)
# floatb -> Floating Base (und auch fixed Base) Modell der Basis.
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# dynamics -> Berechnung der Dynamik
# regressor -> Regressorform (parameterlinear)
#
# Siehe auch: robot_chain_floatb_rotmat_dynamics_regressor.mw.
# Im Gegensatz zu der allgemeinen Dynamik werden hier die fixed-Base-Ausdrücke ohne eine "twist"-Basis-Berechnung übernommen.
# Ursache ist, dass der Parametervektor und damit der Regressor sich zwischen Fixed-Base und Floating-Base unterscheidet und daher ein Ableiten des Fixed-Base falles aus der Floating-Base-Formulierung mit einfacher Basis-Darstellung ("twist") nicht möglich ist.
# Autor
# 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):
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
codegen_act := true:
codegen_opt := 2:
codeexport_grav := true:
codeexport_corvec := true:
codeexport_cormat := true:
codeexport_inertia := true:
codeexport_inertiaD := true:
codeexport_invdyn := true:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../helper/proc_LagrangeN":
read "../helper/proc_index_symmat2vector":
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: # nur zum Abschätzen der Komplexität
;
# Term-Vereinfachungen einstellen
if not assigned(simplify_options) or simplify_options(10)=-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 := 1: # standardmäßig simplify-Befehle anwenden
  end if:
else # Benutzer-Einstellungen:
  use_simplify := simplify_options(10): # zehnter Eintrag ist für Dynamik-Regressor
end if:
# Mit diesem Arbeitsblatt werden die Regressor-Ausdrücke für Fixed-Base und Floating-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):
fi:
# Es ist in diesem Arbeitsblatt möglich, zwei verschiedene Regressoren zu generieren und zu exportieren: Basierend auf Minimalparametern und auf vollem Parametersatz (PV2).
# Der Term "regressor" oder "regressor_minpar" ist jeweils in den Dateinamen enthalten.
# Der folgende Befehl muss immer auf "regressor_minpar" gesetzt sein, da diese Zeile durch das Skript robot_codegen_maple_preparation.sh ausgewertet und modifiziert wird.
regressor_modus := "regressor_minpar":
if regressor_modus = "regressor_minpar" then
  ekinfile := sprintf("../codeexport/%s/tmp/energy_kinetic_%s_regressor_minpar_maple.m", robot_name, expstring):
  if FileTools[Exists](ekinfile) then
    read ekinfile:
  else
    printf("%s. Energie-Regressor in Minimalparameterform wurde nicht berechnet. Abbruch der Dynamik-Minimalparameterregressor-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:
  read ekinfile:
  read sprintf("../codeexport/%s/tmp/energy_potential_%s_regressor_minpar_maple.m", robot_name, expstring):
  t_ges := t_ges_minpar:
  u_ges := u_ges_minpar:
  printf("%s. Generiere Minimalparameterregressor der Dynamik für %s\n", FormatTime("%Y-%m-%d %H:%M:%S"), robot_name):
end if:
if regressor_modus = "regressor" then
  read sprintf("../codeexport/%s/tmp/energy_kinetic_%s_regressor_maple.m", robot_name, expstring):
  read sprintf("../codeexport/%s/tmp/energy_potential_%s_regressor_maple.m", robot_name, expstring):
  t_ges := t_ges:
  u_ges := u_ges:
  printf("%s. Generiere Regressor der Dynamik für %s (nicht Minimalparameter)\n", FormatTime("%Y-%m-%d %H:%M:%S"), robot_name):
end if:
# Schalter zur Auswahl der unterschiedlichen Terme, die exportiert werden sollen. Für parallele Berechnung interessant.
DynString := "Term:":
if codeexport_grav then
  DynString := sprintf("%s g",DynString):
end if:
if codeexport_corvec then
  DynString := sprintf("%s c",DynString):
end if:
if codeexport_cormat then
  DynString := sprintf("%s C",DynString):
end if:
if codeexport_inertia then
  DynString := sprintf("%s M",DynString):
end if:
if codeexport_inertiaD then
  DynString := sprintf("%s MD",DynString):
end if:
if codeexport_invdyn then
  DynString := sprintf("%s tau",DynString):
end if:
printf("%s. Generiere Dynamik-Regressor (%s) für %s und %s\n", FormatTime("%Y-%m-%d %H:%M:%S"), DynString, robot_name, base_method_name):
# Platzhalter-Vektor der Dynamik-Parameter aufstellen
nDP := ColumnDimension(u_ges):
PV := Matrix(nDP,1):
for i from 1 to nDP do
  if regressor_modus = "regressor_minpar" then
    PV(i,1) := parse(sprintf("MDP%d%", i)):
  elif regressor_modus = "regressor" then
    PV(i,1) := parse(sprintf("DP%d%", i)):
  end if:
end do:
PV:
# Name für Export der Terme als Funktion des Parametervektors
if regressor_modus = "regressor_minpar" then
  regshortname := "mdp":
elif regressor_modus = "regressor" then
  regshortname := "dp":
end if:

# Lagrange Formalismus (mit Funktion)
OutputLagrange := LagrangeN(t_ges, u_ges):
dTdqDdt_s := OutputLagrange[1]:
dTdq_s := OutputLagrange[2]:
dUdq_s := OutputLagrange[3]:
save dUdq_s, sprintf("../codeexport/%s/tmp/floatb_lagrange_dUdq_s_%s_maple.m", robot_name, regressor_modus):
save dTdq_s, sprintf("../codeexport/%s/tmp/floatb_lagrange_dTdq_s_%s_maple.m", robot_name, regressor_modus):
save dTdqDdt_s, sprintf("../codeexport/%s/tmp/floatb_lagrange_dTdqDdt_s_%s_maple.m", robot_name, regressor_modus):
# Extraktion einzelner Terme
# Gravitational Load
# Generate
taug_regressor_s := dUdq_s:
# Terme vereinfachen
if codeexport_grav then
  if use_simplify=1 then
    tmp_t1:=time():
    tmp_l1 := length(taug_regressor_s):
    taug_regressor_s := simplify2(taug_regressor_s):
    tmp_t2:=time():
    tmp_l2 := length(taug_regressor_s):
    printf("%s: Gravitationsmoment-Regressor 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:
end if:
save taug_regressor_s, sprintf("../codeexport/%s/tmp/gravload_%s_maple.m", robot_name, regressor_modus):
# Matlab Export
# Belastung der Basis (nur Floating Base)
if codegen_act and codeexport_grav and not(base_method_name="twist") then
  MatlabExport(taug_regressor_s(1..6,..), sprintf("../codeexport/%s/tmp/gravload_base_floatb_%s_%s_matlab.m", robot_name, base_method_name, regressor_modus), codegen_opt):
end if:
# Belastung der Gelenke:
# Fixed-Base: Gravitationsvektor im Basis-KS
# Floating Base: Gravitationsvektor im Welt-KS, Basis-Orientierung berücksichtigt
taug := taug_regressor_s . PV:
if codegen_act and codeexport_grav then
  MatlabExport(taug_regressor_s(7..NQ,..), sprintf("../codeexport/%s/tmp/gravload_joint_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(taug(7..NQ,..), sprintf("../codeexport/%s/tmp/gravload_joint_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:
# Kompletter Vektor
if codegen_act and codeexport_grav and not(base_method_name="twist") then
  MatlabExport(taug_regressor_s(1..NQ,..), sprintf("../codeexport/%s/tmp/gravload_floatb_%s_%s_matlab.m", robot_name, base_method_name, regressor_modus), codegen_opt):
  MatlabExport(taug(1..NQ,..), sprintf("../codeexport/%s/tmp/gravload_floatb_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:

# Mass Matrix
# Berechnung vollständige Massenmatrix
# Generiere die Massenmatrix erst für Floating-Base-Komplettsystem, dann entnehme die Teilmatrizen für Gelenk-Gelenk, Gelenk-Basis zum separaten Export
Paramvec_size := ColumnDimension(t_ges):
# Berechnung vollständige Massenmatrix
# Initialisiere. Speichere nur den unteren linken Teil der Massenmatrix
# Siehe: https://de.wikipedia.org/wiki/Symmetrische_Matrix
tauMM_regressor_s := dTdqDdt_s(1..NQ,..):
MM_regressor_s := Matrix(NQ*(NQ+1)/2, Paramvec_size):
if codeexport_inertia or codeexport_inertiaD or codeexport_cormat then
i_rr := 0:
for i to NQ do # Zeilenindex der Massenmatrix
  for j to NQ do  # Spaltenindex der Massenmatrix
    if j > i then
      next: # rechte obere Seite der symmetrischen Matrix. Keine neue Information. Nicht berechnen oder speichern.
    end if:
    i_rr := i_rr + 1: # Gehe zeilenweise durch den unteren linken Teil der Massenmatrix (inkl. Diagonale)
    for k to Paramvec_size do # Spaltenindex der Regressormatrix
      MM_regressor_s[i_rr, k] := diff(tauMM_regressor_s[i, k], qDD_s[j, 1]):
    end do:
  end do:
end do:
# Terme vereinfachen
if use_simplify>=1 then
  tmp_t1:=time():
  tmp_l1 := length(MM_regressor_s):
  printf("%s. Beginne Vereinfachung: Massenmatrix-Regressor (%s). Länge: %d.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), regressor_modus, tmp_l1):
  MM_regressor_s := simplify2(MM_regressor_s):
  tmp_t2:=time():
  tmp_l2 := length(MM_regressor_s):
  printf("%s. Massenmatrix-Regressor (%s) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), regressor_modus, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
end if:
end if:
# Export Gesamt-Massenmatrix
if codegen_act and codeexport_inertia and not(base_method_name="twist") then
  MatlabExport(MM_regressor_s, sprintf("../codeexport/%s/tmp/inertia_floatb_%s_%s_matlab.m", robot_name, base_method_name, regressor_modus), codegen_opt):
end if:
# Gelenk-Massenmatrix extrahieren
MMjj_regressor_s := Matrix(NQJ*(NQJ+1)/2, Paramvec_size):
itmp:=0:
for i to NQ do
  for j to NQ do  # Spaltenindex der Massenmatrix
    if j > i then
      next: # rechte obere Seite der symmetrischen Matrix. Keine neue Information. Nicht berechnen oder speichern.
    end if:
    if i > 6 and j > 6 then # unterer rechter Teil (Gelenkmoment-Gelenkbeschleunigung-Terme)
      i_MM := index_symmat2vec(NQ,i,j): # Passender Index für zeilenweise ausgewählten symmetrischen Teil (siehe Gesamt-Massenmatrix)
      itmp := itmp + 1:
      MMjj_regressor_s[itmp,..] := MM_regressor_s[i_MM,..]:
    end if:
  end do:
end do:
# Multiplikation der Massenmatrix mit dem Regressorvektor und Export als obere Dreiecksmatrix
MMjj_tmp := MMjj_regressor_s . PV: # Vektor der oberen rechten Dreiecksmatrix
;
# Export Gelenkmassenmatrix
if codegen_act and codeexport_inertia and base_method_name="twist" then
  MatlabExport(MMjj_regressor_s, sprintf("../codeexport/%s/tmp/inertia_joint_joint_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(MMjj_tmp, sprintf("../codeexport/%s/tmp/inertia_joint_joint_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:
# Gelenk-Basis-Massenmatrix
#  (untere linke Teilmatrix der Gesamt-Massenmatrix)
MMjb_regressor_s := Matrix(6*NQJ, Paramvec_size):
# Gehe Schleife über alle Massenmatrix-Elemente durch und entnehme die passenden Elemente für die Teilmatrix)
itmp:=0:
for i to NQ do
  for j to NQ do  # Spaltenindex der Massenmatrix
    if i > 6 and j < 7 then # unterer linker Teil
      i_MM := index_symmat2vec(NQ,i,j): # Passender Index für zeilenweise ausgewählten symmetrischen Teil (siehe Gesamt-Massenmatrix)
      itmp := itmp + 1:
      MMjb_regressor_s[itmp,..] := MM_regressor_s[i_MM,..]:
    end if:
  end do:
end do:
if codegen_act and codeexport_inertia and not(base_method_name="twist") then
  MatlabExport(MMjb_regressor_s, sprintf("../codeexport/%s/tmp/inertia_joint_base_floatb_%s_%s_matlab.m", robot_name, base_method_name, regressor_modus), codegen_opt):
end if:
# Basis-Massenmatrix
MMbb_regressor_s := Matrix(6*(6+1)/2, Paramvec_size):
# Gehe Schleife über alle Massenmatrix-Elemente durch und entnehme die passenden Elemente für die Teilmatrix)
itmp:=0:
for i to NQ do
  for j to NQ do  # Spaltenindex der Massenmatrix
    if j > i then
      next: # rechte obere Seite der symmetrischen Matrix. Keine neue Information. Nicht berechnen oder speichern.
    end if:
    if i < 7 and j < 7 then # unterer linker Teil
      i_MM := index_symmat2vec(NQ,i,j): # Passender Index für zeilenweise ausgewählten symmetrischen Teil (siehe Gesamt-Massenmatrix)
      itmp := itmp + 1:
      MMbb_regressor_s[itmp,..] := MM_regressor_s[i_MM,..]:
    end if:
  end do:
end do:
if codegen_act and codeexport_inertia and not(base_method_name="twist") then
  MatlabExport(MMbb_regressor_s, sprintf("../codeexport/%s/tmp/inertia_base_base_floatb_%s_%s_matlab.m", robot_name, base_method_name, regressor_modus), codegen_opt):
end if:
# Mass Matrix Time Derivative
# Gesamt Massenmatrix
MM_regressor_t := convert_s_t(MM_regressor_s):
MMD_regressor_t := diff~(MM_regressor_t, t):
MMD_regressor_s := convert_t_s(MMD_regressor_t):
if codegen_act and codeexport_inertiaD and not(base_method_name="twist") then
  MatlabExport(MMD_regressor_s, sprintf("../codeexport/%s/tmp/inertiaD_floatb_%s_%s_matlab.m", robot_name, base_method_name, regressor_modus), codegen_opt):
end if:
# Konvertiere Gelenk-Massenmatrix in zeitabhängige Variablen, um Zeitableitung zu berechnen
MMjj_regressor_t := convert_s_t(MMjj_regressor_s):
MMDjj_regressor_t := diff~(MMjj_regressor_t, t):
MMDjj_regressor_s := convert_t_s(MMDjj_regressor_t):
if codegen_act and codeexport_inertiaD and base_method_name="twist" then
  MatlabExport(MMDjj_regressor_s, sprintf("../codeexport/%s/tmp/inertiaD_joint_joint_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
end if:
# Gelenk-Basis-Massenmatrix
MMjb_regressor_t := convert_s_t(MMjb_regressor_s):
MMDjb_regressor_t := diff~(MMjb_regressor_t, t):
MMDjb_regressor_s := convert_t_s(MMDjb_regressor_t):
if codegen_act and codeexport_inertiaD and not(base_method_name="twist") then
  MatlabExport(MMDjb_regressor_s, sprintf("../codeexport/%s/tmp/inertiaD_joint_base_floatb_%s_%s_matlab.m", robot_name, base_method_name, regressor_modus), codegen_opt):
end if:
# Coriolis Vector
# Generieren (gleiches Vorgehen für fixed und floating base)
tauC_regressor_s := dTdqDdt_s-dTdq_s:
for i to NQ do
  tauC_regressor_s := subs({qDD_s(i, 1) = 0}, tauC_regressor_s):
end do:
save tauC_regressor_s, sprintf("../codeexport/%s/tmp/coriolisvec_joint_%s_%s_maple.m", robot_name, expstring, regressor_modus):
tauc := tauC_regressor_s . PV:
# Matlab Export
# Belastung der Gelenke
if codegen_act and codeexport_corvec then
  MatlabExport(tauC_regressor_s(7..NQ,..), sprintf("../codeexport/%s/tmp/coriolisvec_joint_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(tauc(7..NQ,..), sprintf("../codeexport/%s/tmp/coriolisvec_joint_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:
# Gesamter Vektor für Floating Base
if codegen_act and codeexport_corvec and not(base_method_name="twist") then
  MatlabExport(tauC_regressor_s(1..NQ,..), sprintf("../codeexport/%s/tmp/coriolisvec_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(tauc(1..NQ,..), sprintf("../codeexport/%s/tmp/coriolisvec_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:
# Nur Basis-Terme
if codegen_act and codeexport_corvec and not(base_method_name="twist") then
  MatlabExport(tauC_regressor_s(1..6,..), sprintf("../codeexport/%s/tmp/coriolisvec_base_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(tauc(1..6,..), sprintf("../codeexport/%s/tmp/coriolisvec_base_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:
# Coriolis Matrix
# Floating-Base Coriolismatrix (Gesamt)
# Calculation with Christoffel Symbol approach
# [KhalilDombre2002], equ. (9.7) (p. 195)
# Funktion zur Anwendung des Algorithmus aus [KhalilDombre2002]:
cijk := proc (i::integer, j::integer, k::integer, A, qs)
  local c:
  c := (1/2)*(diff(A[i, j], qs(k, 1)))+(1/2)*(diff(A[i, k], qs(j, 1)))-(1/2)*(diff(A[j, k], qs(i, 1))):
  return c:
end proc:
# Initialisierung. Speichere die vollständige Coriolismatrix (nicht symmetrisch/schiefsymmetrisch). Unterschied zum Vorgehen bei der Massenmatrix
C_regressor_s := Matrix(NQ*NQ, Paramvec_size):
# Berechnung
if codeexport_cormat then:
i_rr := 0: # Vektor-Index für den Regressor der Coriolismatrix (Ausgabe)
for i to NQ do # Zeilenindex der Coriolismatrix
  for j to NQ do  # Spaltenindex der Coriolismatrix
    i_rr := i_rr + 1: # Gehe zeilenweise durch den unteren linken Teil der Coriolismatrix (inkl. Diagonale)
    for k from 1 to Paramvec_size do # Spaltenindex der Regressormatrix
      # Massenmatrix für Parameter k generieren (für Funktion mit Christoffel-Symbol-Ansatz benötigt)
      MM_k := Matrix(NQ,NQ):
      for ii from 1 to NQ do
        for jj from 1 to NQ do
          MM_k(ii,jj) := MM_regressor_s(index_symmat2vec(NQ, ii, jj), k):
        end do:
      end do:
      for l from 1 to NQ do # Siehe [KhalilDombre2002]
        C_regressor_s[i_rr, k] := C_regressor_s[i_rr,k] + cijk(i,j,l,MM_k,q_s)*qD_s[l,1]:
      end do:
    end do:
  end do:
end do:
# Terme vereinfachen
if use_simplify>=1 then
  tmp_t1:=time():
  tmp_l1 := length(C_regressor_s):
  printf("%s. Beginne Vereinfachung: Coriolis-Matrix-Regressor (%s). Länge: %d.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), regressor_modus, tmp_l1):
  C_regressor_s := simplify2(C_regressor_s):
  tmp_t2:=time():
  tmp_l2 := length(C_regressor_s):
  printf("%s: Coriolis-Matrix-Regressor (%s) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), regressor_modus, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
end if:
end if:
save C_regressor_s, sprintf("../codeexport/%s/tmp/coriolismat_%s_%s_maple.m", robot_name, expstring, regressor_modus):
# Matlab Export: Floating Base Gesamt
if codegen_act and codeexport_cormat and not(base_method_name="twist") then
  MatlabExport(C_regressor_s, sprintf("../codeexport/%s/tmp/coriolismat_floatb_%s_%s_matlab.m", robot_name, base_method_name, regressor_modus), codegen_opt):
end if:
# Gelenk-Coriolismatrix
# Extrahiere Teilmatrix, die für die Gelenkterme (ohne Basiseinfluss) zuständig sind
Cjj_regressor_s := Matrix(NQJ*NQJ, Paramvec_size):
itmp := 0:
for i to NQ do # Zeilenindex der Coriolismatrix
  for j to NQ do  # Spaltenindex der Coriolismatrix
    if i > 6 and j > 6 then # unterer rechter Teil
      i_C := (i-1)*NQ+j: # Passender Index für zeilenweise ausgewähltes Element
      itmp := itmp + 1:
      Cjj_regressor_s[itmp,..] := C_regressor_s[i_C,..]:
    end if:
  end do:
end do:
if codegen_act and codeexport_cormat and base_method_name="twist" then
  MatlabExport(Cjj_regressor_s, sprintf("../codeexport/%s/tmp/coriolismat_joint_fixb_%s_matlab.m", robot_name, regressor_modus), codegen_opt):
end if:
# Inverse Dynamics
# Generate
tau_regressor_s := dTdqDdt_s-dTdq_s+dUdq_s:
save tau_regressor_s, MMjj_regressor_s, tauMM_regressor_s, tauC_regressor_s, taug_regressor_s, sprintf("../codeexport/%s/tmp/invdyn_%s_%s_maple.m", robot_name, expstring, regressor_modus):
tau := tau_regressor_s . PV:
# Gesamter Vektor (floating base)
if codegen_act and codeexport_invdyn and not(base_method_name="twist") then
  MatlabExport(tau_regressor_s(1..NQ,..), sprintf("../codeexport/%s/tmp/invdyn_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(tau, sprintf("../codeexport/%s/tmp/invdyn_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:
# Belastung der Basis (floating base)
if codegen_act and codeexport_invdyn and not(base_method_name="twist") then
  MatlabExport(tau_regressor_s(1..6,..), sprintf("../codeexport/%s/tmp/invdyn_base_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(tau(1..6,..), sprintf("../codeexport/%s/tmp/invdyn_base_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:
# Belastung der Gelenke
if codegen_act and codeexport_invdyn then
  MatlabExport(tau_regressor_s(7..NQ,..), sprintf("../codeexport/%s/tmp/invdyn_joint_%s_%s_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(tau(7..NQ,..), sprintf("../codeexport/%s/tmp/invdyn_joint_%s_%s_matlab.m", robot_name, expstring, regshortname), codegen_opt):
end if:
# Regressor-Matrix als Vektor hinschreiben. Dabei Null-Einträge herausfinden und nicht weiter berücksichtigen.
# Dadurch wird die obere rechte Dreiecksform der Regressor-Matrix ausgenutzt.
# Platzhalter-Vektor für alle Einträge der Regressor-Matrix
Reg_Vector := Matrix(ColumnDimension(tau_regressor_s)*RowDimension(tau_regressor_s), 1):
# Gelenkmoment in Abhängigkeit des Regressor-Vektors (RV)
tauJ_RV := Matrix(NQJ, 1):
kk := 0: # Zähler für laufende Nummer des Regressor-Vektors
for i from 1 to ColumnDimension(tau_regressor_s) do
  for j from 1 to RowDimension(tau_regressor_s)-6 do
    # Gehe Spalten- und dann Zeilenweise durch die Regressormatrix (nur Gelenk-Teil)
    if tau_regressor_s(j+6,i) <> 0 then # Eintrag ungleich Null
    	 kk := kk + 1:
    	 # Regressor-Vektor-Eintrag als Platzhalter direkt so, wie er in Matlab-Funktion gebraucht wird ("RV" ist Eingabe der Matlab-Funktion).
    	 rv_kk := parse(sprintf("RV(%d)", kk)):
    	 # Eintrag für Regressor-Vektor so schreiben, dass er in einer Matlab-Funktion aus der Regressor-Matrix gebildet werden kann
    	 Reg_Vector(kk,1) := parse(sprintf("RM(%d,%d)", j, i)):
    	 # Gelenkmoment aus Multiplikation mit Parametervektor. Dadurch Berechnung unter Ausnutzung der Nullen.
    	 tauJ_RV(j) := tauJ_RV(j) + rv_kk*PV(i):
    end if:
  end do:
end do:
# Kürzen des Regressor-Vektors auf die tatsächlich benutzten Einträge
Reg_Vector := Reg_Vector(1..kk,..):
save Reg_Vector, sprintf("../codeexport/%s/tmp/invdyn_joint_%s_%s_occupancy_vector_maple", robot_name, expstring, regressor_modus):
if codegen_act and codeexport_invdyn then
  interface(warnlevel=0): # Unterdrücke die folgende Warnung (Ist prinzipbedingt, da RM/RV als Funktionsnamen interpretiert werden. Der Code funktioniert trotzdem.
  MatlabExport(Reg_Vector, sprintf("../codeexport/%s/tmp/invdyn_joint_%s_%s_occupancy_vector_matlab.m", robot_name, expstring, regressor_modus), codegen_opt):
  MatlabExport(tauJ_RV, sprintf("../codeexport/%s/tmp/invdyn_joint_%s_%s_mult_matlab.m", robot_name, expstring, regshortname), codegen_opt):
  interface(warnlevel=3):
end if:


# This file sets up the 15-vertex triangulation of CP^2 introduced by
# Gaifullin in "A minimal triangulation of complex projective plane ..."
# (arxiv:0904.4222)

with(GroupTheory):

CP2_complex := proc()
 local T,V,F,p,a,b,d,s,S,om,ob,rt,
  id_S,o_S,inv_S,act_S,eq_S,eq_X,v,ix_v,i;

 T := table():

 # We take the vertices to be 1,...,15
 # In Gaifullin's presentation, the vertex set is
 # {transposition pairs} union {1,2,3,4} x {1,2,3}
 # Gaifullin's (i,j) is our 1 + 3 * (i-1) + (j-1)
 # The transposition pairs are our vertices 13-15.

 V := {seq(i,i=1..15)}:

 T["vertices"] := V;

 F := NULL:

 # This table encodes the three transposition pairs on {0,1,2,3}
 p := table():
 p[1,0] := 1; p[1,1] := 0; p[1,2] := 3; p[1,3] := 2;
 p[2,0] := 2; p[2,1] := 3; p[2,2] := 0; p[2,3] := 1;
 p[3,0] := 3; p[3,1] := 2; p[3,2] := 1; p[3,3] := 0;

 for a from 1 to 3 do
  for b[0] from 0 to 2 do
   for b[1] from 0 to 2 do
    for b[2] from 0 to 2 do
     for b[3] from 0 to 2 do
      if b[0] <> b[p[a,0]] and
	 b[1] <> b[p[a,1]] and
	 b[2] <> b[p[a,2]] and
	 b[3] <> b[p[a,3]] then
       F := F,{b[0]+1,b[1]+4,b[2]+7,b[3]+10,a+12};
      fi;
     od:
    od:
   od:
  od:
 od:
 F := {F}:

 T["max_simplices"] := F;
 T["all_simplices"] :=
   map(f -> op(combinat[powerset](f)),F) minus {{}}:

 T["simplices"] := table():
 for d from 0 to 4 do
  T["simplices"][d] := select(u -> nops(u) = d+1,T["simplices"]);
 od:

 s := table():
 T["s"] := eval(s);

 s[0] := Perm([]):
 s[1] := Perm([[ 1, 4],[ 2, 5],[ 3, 6],[14,15]]):
 s[2] := Perm([[ 4, 7],[ 5, 8],[ 6, 9],[13,14]]):
 s[3] := Perm([[ 7,10],[ 8,11],[ 9,12],[14,15]]):
 s[4] := Perm([[ 1, 2],[ 4, 5],[ 7, 8],[10,11]]):
 s[5] := Perm([[ 2, 3],[ 5, 6],[ 8, 9],[11,12]]):

 T["S4"] := Group(s[1],s[2],s[3]);
 T["S3"] := Group(s[4],s[5]);
 T["G"]  := Group(seq(s[i],i=1..5));

 S := table():
 T["S"] := eval(S);

 om := exp( 2*Pi*I/3);
 ob := exp(-2*Pi*I/3);
 rt := sqrt(3);

 S[0] := [ 1,[[ 1, 0, 0],[ 0, 1, 0],[ 0, 0, 1]]]:
 S[1] := [-1,[[ 0, 1, 0],[ 1, 0, 0],[ 0, 0, 1]]];
 S[2] := [-1,[[ 1, 0, 0],[ 0, 0, 1],[ 0, 1, 0]]];
 S[3] := [-1,[[ 0,-1, 0],[-1, 0, 0],[ 0, 0, 1]]];
 S[4] := [-1,[[ 1, 0, 0],[ 0, 1, 0],[ 0, 0, 1]]];
 S[5] := [-1,[[ 1, 0, 0],[ 0,om, 0],[ 0, 0,ob]]];

 id_S := S[0];

 o_S := proc(u,v)
  local m,n,mn,A,B,AB;
  m,A := op(u);
  n,B := op(v);
  mn := m*n;
  if n = -1 then A := conjugate(A); fi;
  AB := convert(Matrix(A) . Matrix(B),listlist);
  AB := simplify(expand(rationalize(AB)));
  return [mn,AB];
 end:

 inv_S := proc(u)
  local m,A;
  m,A := op(u);
  if m = -1 then A := conjugate(A); fi;
  A := convert(1/Matrix(A),listlist);
  A := simplify(expand(rationalize(A)));
  return [m,A];
 end:

 act_S := (u) -> proc(x)
  local y;
  y := Vector(x);
  y := simplify(convert(Matrix(u[2]) . y,list));
  if u[1] = -1 then y := map(conjugate,y); fi;
  return y;
 end:

 eq_S := proc(u,v)
  local w;
  if u[1] <> v[1] then return false; fi;
  w := o_S(u,inv_S(v));
  if w[2][1,1] = 0 then return false; fi;
  w := [w[1],map(simplify,map(expand,w[2] /~ w[2][1,1]))];
  return evalb(w = id_S);
 end:

 eq_X := proc(u,v)
  local i,j;
  `and`(seq(seq(evalb(simplify(u[i]*v[j]-u[j]*v[i])=0),j=i+1..3),i=1..2));
 end:

 v := table([
   1 = [  1,-om,-ob],
   2 = [  1,-ob,-om],
   3 = [  1, -1, -1],
   4 = [  1,-om, ob],
   5 = [  1,-ob, om],
   6 = [  1, -1,  1],
   7 = [  1, om,-ob],
   8 = [  1, ob,-om],
   9 = [  1,  1, -1],
  10 = [  1, om, ob],
  11 = [  1, ob, om],
  12 = [  1,  1,  1],
  13 = [  0,  0, rt],
  14 = [  0, rt,  0],
  15 = [ rt,  0,  0]
 ]):

 T["projective_embedding"] := eval(v);

 ix_v := proc(u)
  local i;
  for i from 1 to 15 do
   if eq_X(u,v[i]) then return i; fi;
  od;
  return FAIL;
 end:

 return eval(T):
end:


# A raw simplicial complex is a set of nonempty finite sets, no two
# of which are  contained in each other.  We take this as representing
# a simplicial complex, with the specified sets as the maximal simplices.
#
# A (cooked) simplicial complex is a table with various fields which
# gives a more structured representation of the data.

`is_element/raw_simplicial_complexes` := proc(K)
 global reason;
 local n,i,j;

 if not(type(K,set(set)) or type(K,set(list)) or type(K,list(set)) or type(K,list(list))) then
  reason := ["is_element/raw_simplicial_complexes","K is not a set/list of set/lists",K];
  return false;
 fi;

 if member({},K) or member([],K) then
  reason := ["is_element/raw_simplicial_complexes","K contains the empty set",K];
  return false;
 fi;

 n := nops(K);

 for i from 1 to n-1 do
  for j from i+1 to n do
   if {op(K[i])} minus {op(K[j])} = {} then
    reason := ["is_element/raw_simplicial_complexes","K[i] and K[j] are nested",i,j,K];
    return false;
   fi;
  od;
 od;

 return true;
end:

`vertices/raw_simplicial_complex` := (K) -> map(op,{op(K)});

`is_simplex/raw_simplicial_complex` := proc(K,s)
 local s0,u;

 if not(type(s,{list,set})) then return false; fi;

 s0 := {op(s)};
 if s0 = {} then return false; fi;

 for u in K do
  if s0 minus {op(u)} = {} then
   return true;
  fi;
 od:
 return false;
end:

`is_element/simplicial_complexes` := proc(T)
 if not(type(T,table)) then
  return false;
 fi;

 if not(`is_element/raw_simplicial_complexes`(T["max_simplices"])) then
  return false;
 fi;

 if T["vertices"] <> `vertices/raw_simplicial_complex`(T["max_simplices"]) then
  return false;
 fi;

 return true;
end:

`is_simplex/simplicial_complex` := proc(T,s)
 return `is_simplex/raw_simplicial_complex`(T["max_simplices"],s);
end:

`all_simplices/raw_simplicial_complex` := proc(K)
 local V,K0;
 V := `vertices/raw_simplicial_complex`(K);
 K0 := map(s -> {op(s)},{op(K)});
 K0 := map(op,map(combinat[powerset],K0)) minus {{}};
 K0 := map(s -> sort([op(s)]),K0);
 return K0;
end;

`normalise/raw_simplicial_complex` := proc(K)
 local K0,K1,n,i,j,ok;
 K0 := [op(K)];
 K0 := map(s -> {op(s)},K0);
 K0 := sort(K0,(s,t) -> nops(s) <= nops(t));
 n := nops(K0);
 K1 := NULL;
 for i from 1 to n do
  ok := true;
  for j from i + 1 to n do
   if K0[i] minus K0[j] = {} then
    ok := false;
    break;
   fi;
  od;
  if ok then K1 := K1,K0[i]; fi;
 od;
 K1 := [K1];
 K1 := map(s -> sort([op(s)]),K1);
 return K1;
end:

`cook/raw_simplicial_complex` := proc(K)
 local T;

 T := table();
 T["vertices"] := `vertices/raw_simplicial_complex`(K);
 T["max_simplices"] := K;

 return eval(T);
end;

`edges/raw_simplicial_complex` := proc(K)
 local i,j,s;
 {seq(seq(seq({s[i],s[j]},j=i+1..nops(s)),i=1..nops(s)),s in K)};
end:

`faces/raw_simplicial_complex` := proc(K)
 local i,j,k,s;
 {seq(seq(seq(seq({s[i],s[j],s[k]},k=j+1..nops(s)),j=i+1..nops(s)),i=1..nops(s)),s in K)};
end:

`components/simplicial_complex` := proc(T)
 local P,Q,R,s,v;

 P := {seq({v},v in T["vertices"])};
 for s in T["max_simplices"] do
  Q,R := selectremove(c -> c intersect {op(s)} = {},P);
  P := {map(op,R),op(Q)};
 od:
 T["components"] := P;
 return P;
end:

`reindex/simplicial_complex` := proc(T,f)
 local T0,v;

 T0 := table();
 T0["vertices"] := map(f,T["vertices"]);
 T0["max_simplices"] := map(s -> map(f,s),T["max_simplices"]);

 if member(["embedding"],[indices(T)]) then
  T0["embedding_dim"] = T["embedding_dim"];
  for v in T["vertices"] do
   T0["embedding"][f(v)] := T["embedding"][v];
  od:
 fi;

 return eval(T0);
end:

`set_edges/simplicial_complex` := proc(T,force := false)
 if type(T["edges"],{list,set}) and not force then
  return T["edges"];
 fi;
 T["edges"] := `edges/raw_simplicial_complex`(T["max_simplices"]);
 return T["edges"];
end:

`set_faces/simplicial_complex` := proc(T,force := false)
 if type(T["faces"],{list,set}) and not force then
  return T["faces"];
 fi;
 T["faces"] := `faces/raw_simplicial_complex`(T["max_simplices"]);
 return T["faces"];
end:

`set_all_simplices/simplicial_complex` := proc(T,force := false)
 local K,d,i;

 if (type(T["all_simplices"],{list,set}) and not(force)) then
  return T["all_simplices"];
 fi;

 K := T["max_simplices"];
 T["all_simplices"] := `all_simplices/raw_simplicial_complex`(K);
 d := `dim/raw_simplicial_complex`(K);
 T["dim"] := d;
 T["simplices_by_dim"] := table():
 for i from 0 to d do
  T["simplices_by_dim"][i] :=
   select(s -> nops(s) = i + 1,T["all_simplices"]);
 od:
 return T["all_simplices"];
end:

`dim/raw_simplicial_complex` := (K) -> max(map(nops,K)) - 1;

`dim/simplicial_complex` := proc(T)
 local d;
 d := `dim/raw_simplicial_complex`(T["max_simplices"]);
 T["dim"] := d;
 return d;
end:

`euler_characteristic/raw_simplicial_complex` := proc(K)
 local L;

 L := `all_simplices/raw_simplicial_complex`(K);
 L := [op(L)];
 L := map(s -> (-1)^(nops(s)+1),L);
 return `+`(op(L));
end:

`euler_characteristic/simplicial_complex` := proc(T)
 local L;

 if not(type(T["all_simplices"],{set,list})) then
  T["all_simplices"] := `all_simplices/raw_simplicial_complex`(T["max_simplices"]);
 fi;

 L := T["all_simplices"];
 L := [op(L)];
 L := map(s -> (-1)^(nops(s)+1),L);
 return `+`(op(L));
end:

`has_embedding/simplicial_complex` := proc(T)
 if type(T["embedding"],table) then
  if not(type(T["embedding_dim"],nonnegint))
     and nops(T["vertices"]) > 0 then
   T["embedding_dim"] := nops(T["embedding"][T["vertices"][1]]);
  fi;
  return true;
 else
  return false;
 fi;
end:

`has_normal/simplicial_complex` := proc(T)
 if not(`has_embedding/simplicial_complex`(T)) then
  return false;
 fi;

 return type(T["normal"],table);
end:

# Condensation replaces a raw simplicial complex by an equivalent one in
# which the vertex set has the form {1,...,n} for some n.

`is_condensed/raw_simplicial_complex` := proc(K)
 local V,n,i;
 V := `vertices/raw_simplicial_complex`(K);
 n := nops(V);
 return evalb(V = {seq(i,i=1..n)});
end;

`condense/raw_simplicial_complex` := proc(K)
 local V,ix,n,i,L;
 V := `vertices/raw_simplicial_complex`(K);
 n := nops(V);
 for i from 1 to n do
  ix[V[i]] := i;
 od:
 L := map(s -> map(v -> ix[v],s),K);
 return L;
end:

`is_condensed/simplicial_complex` := proc(T)
 local V,n,i;
 V := {op(T["vertices"])};
 n := nops(V);
 return evalb(V = {seq(i,i=1..n)});
end;

`condense/simplicial_complex` := proc(T)
 local V,T0,n,i;

 T0 := table():
 V := T["vertices"];
 n := nops(V);

 T0["max_simplices"] := `condense/raw_simplicial_complex`(T["max_simplices"]);
 T0["vertices"] := {seq(i,i=1..n)};

 if `has_embedding/simplicial_complex`(T) then
  T0["embedding_dim"] := T["embedding_dim"];
  T0["embedding"] := table();
  for i from 1 to n do
   T0["embedding"][i] := T["embedding"][V[i]];
  od;

  if `has_normal/simplicial_complex`(T) then
   T0["normal"] := table():
   for i from 1 to n do
    T0["normal"][i] := T["normal"][V[i]];
   od;
  fi;
 fi;

 return eval(T0);
end:

`clone/simplicial_complex` := proc(T)
 local T0,k,keys,i;

 T0 := table():
 keys := [
  "vertices",
  "edges",
  "faces",
  "max_simplices",
  "all_simplices",
  "dim",
  "embedding_dim",
  "embedding",
  "normal"
 ];

 for k in keys do
  if member([k],[indices(T)]) then
   if type(T[k],table) then
    T0[k] := table():
    for i in [indices(T[k])] do
     T0[k][op(i)] := T[k][op(i)];
    od:
   else
    T0[k] := T[k];
   fi;
  fi;
 od:

 return eval(T0);
end:

`restrict/simplicial_complex` := proc(T,V0)
 local V1,T0,K0,P0,P,v;

 V1 := {op(V0)};
 if V1 minus {op(T["vertices"])} <> {} then
  error "V0 is not a subset of the vertex set";
 fi;

 T0 := table();
 T0["vertices"] := V0;
 K0 := map(s -> {op(s)} intersect V1,T["max_simplices"]);
 K0 := `normalise/raw_simplicial_complex`(K0);
 T0["max_simplices"] := K0;

 P := T["embedding"];
 if type(P,table) then
  T0["embedding_dim"] := T["embedding_dim"];
  P0 := table():
  for v in V0 do
   P0[v] := P[v];
  od:
  T0["embedding"] := eval(P0);
 fi;

 return eval(T0);
end:

`disjoint_union/simplicial_complex` := proc(T,U)
 local n,U0,TU,v;

 if `is_condensed/simplicial_complex`(T) and
    `is_condensed/simplicial_complex`(U) then
  n := nops(T["vertices"]);
  U0 := `reindex/simplicial_complex`(U,i -> i + n);
 else
  if {op(T["vertices"])} intersect {op(U["vertex_set"])} <> {} then
   error "Vertex sets are not disjoint";
  fi;
  U0 := eval(U);
 fi;

 TU := table();
 TU["vertices"] := [op(T["vertices"]),op(U0["vertices"])];
 TU["max_simplices"] := [op(T["max_simplices"]),op(U0["max_simplices"])];

 if `has_embedding/simplicial_complex`(T) and
    `has_embedding/simplicial_complex`(U0) and
    T["embedding_dim"] = U0["embedding_dim"] then
  TU["embedding_dim"] := T["embedding_dim"];
  TU["embedding"] := table();
  for v in T["vertices"] do
   TU["embedding"][v] := T["embedding"][v];
  od;
  for v in U0["vertices"] do
   TU["embedding"][v] := U0["embedding"][v];
  od;
 fi;

 return eval(TU);
end:

`join/simplicial_complex` := proc(T,U)
 local n,m,U0,TU,s,t,v;

 if `is_condensed/simplicial_complex`(T) and
    `is_condensed/simplicial_complex`(U) then
  n := nops(T["vertices"]);
  U0 := `reindex/simplicial_complex`(U,i -> i + n);
 else
  if {op(T["vertices"])} intersect {op(U["vertex_set"])} <> {} then
   error "Vertex sets are not disjoint";
  fi;
  U0 := eval(U);
 fi;

 TU := table();
 TU["vertices"] := [op(T["vertices"]),op(U0["vertices"])];
 TU["max_simplices"] := [
  op(T["max_simplices"]),
  op(U0["max_simplices"]),
  seq(seq([op(s),op(t)],t in U0["max_simplices"]),s in T["max_simplices"])
 ];

 if `has_embedding/simplicial_complex`(T) and
    `has_embedding/simplicial_complex`(U0) then
  n := T["embedding_dim"];
  m := U0["embedding_dim"];
  TU["embedding_dim"] := n+m+1;
  TU["embedding"] := table();
  for v in T["vertices"] do
   TU["embedding"][v] := [op(T["embedding"][v]),0$(m+1)];
  od;
  for v in U0["vertices"] do
   TU["embedding"][v] := [0$n,1,op(U0["embedding"][v])];
  od;
 fi;

 return eval(TU);
end:

# The following function checks whether we have an embedding of a raw
# simplicial complex K in R^d.  The argument d is the embedding dimension.
# The argument a should be a list or table, so that a[i] is an element
# of R^d for each index i appearing in K.

`is_embedding/raw_simplicial_complex` := proc(K,d,a)
 global reason;
 local m,Kl,Ka,i,j,k,eqd,bc,rr,AA,BB,u,v,w,p,M;

 m := nops(K);
 Kl := [op(K)];
 Ka := map(s -> [seq(a[i],i in s)],Kl);

 for u in Ka do
  if not `is_element/simplices`(d)(u) then
   reason := ["is_embedding/simplicial_complex","u is not a simplex in R^d",u,d,reason];
   return false;
  fi;
 od;

 eqd := evalb({d+1,op(map(nops,Kl))} = {d+1});

 bc := map(`barycentre/simplex`(d),Ka):
 rr := map(`radius/simplex`(d),Ka):

 if eqd then
  AA := map(`primal_matrix/simplex`(d),Ka):
  BB := map(A -> 1/A,AA):
 fi;

 for i from 1 to m-1 do
  for j from i+1 to m do
   u := Ka[i];
   v := Ka[j];
   p := {op(K[i])} intersect {op(K[j])};
   w := {seq(a[k],k in p)};
   if not(nops(w) = nops(p) and
          w = {op(u)} intersect {op(v)}) then
    return false;
   fi;
   if evalf(`d_2/R`(d)(bc[i],bc[j]) - rr[i] - rr[j]) <= 0 then
    if eqd then
     M := <BB[i].AA[j],BB[j].AA[j]>;
     if min(seq(max(seq(M[i,j],j=1..d+1)),i=1..2*d+2)) >= 0 then
      if not(`intersect_nicely/simplices`(d)(u,v)) then
       reason := ["is_embedding/simplicial_complex","bad intersection",u,v];
       return false;
      fi;
     fi;
    else
     if not(`intersect_nicely/simplices`(d)(u,v)) then
      reason := ["is_embedding/simplicial_complex","bad intersection",u,v];
      return false;
     fi;
    fi;
   fi;
  od;
 od;

 return true;
end:

`star/simplicial_complex` := proc(T,s)
 local K,P,K0,T0,P0,s0,v;

 K := T["max_simplices"];
 K := map(u -> {op(u)},{op(K)});
 if `is_simplex/raw_simplicial_complex`(K,s) then
  s0 := {op(s)};
 elif member(s,T["vertices"]) then
  s0 := {s};
 else
  error "Second argument is not a vertex or simplex";
 fi;

 K0 := select(u -> s0 minus u = {},K);
 T0 := `cook/raw_simplicial_complex`(K0);

 P := T["embedding"];
 if type(P,table) then
  T0["embedding_dim"] := T["embedding_dim"];
  P0 := table():
  for v in T0["vertices"] do
   P0[v] := P[v];
  od:
  T0["embedding"] := eval(P0);
 fi;

 return eval(T0);
end:

`link/simplicial_complex` := proc(T,s)
 local K,s0,T0,T1;

 K := T["max_simplices"];
 K := map(u -> {op(u)},{op(K)});
 if `is_simplex/raw_simplicial_complex`(K,s) then
  s0 := {op(s)};
 elif member(s,T["vertices"]) then
  s0 := {s};
 else
  error "Second argument is not a vertex or simplex";
 fi;

 T0 := `star/simplicial_complex`(T,s);
 T1 := `restrict/simplicial_complex`(T0,{op(T0["vertices"])} minus s0);
 return eval(T1);
end:

# For a connected one-manifold (possibly with boundary), list the
# vertices in natural order.  If the complex is a circle, then
# the first vertex in the list is repeated at the end.

`track/simplicial_complex` := proc(T,v_)
 local V,K,L,E,v,w,t,xx;

 if nops(T["vertices"]) <= 1 then
  return [op(T["vertices"])];
 fi;

 if `dim/simplicial_complex`(T) > 1 then
  error "Dimension is bigger than one";
 fi;

 if nops(`components/simplicial_complex`(T)) > 1 then
  error "Complex is not connected";
 fi;

 V := T["vertices"];
 if type(V,set) then V := sort([op(V)]); fi;

 K := {op(map(s -> {op(s)},T["max_simplices"]))};
 L := table();

 for v in V do
  L[v] := select(u -> u <> v and member({u,v},K),V);
 od;

 if max(seq(nops(L[v]),v in V)) > 2 then
  error "Complex is branched";
 fi;

 E := select(v -> nops(L[v]) = 1, V);
 if E = [] then
  if nargs > 1 then
   if member(v_,V) then
    v := v_;
   else
    error "Invalid starting point";
   fi;
  else
   v := V[1];
  fi;
 else
  if nargs > 1 then
   if member(v_,E) then
    v := v_;
   else
    error "Invalid starting point";
   fi;
  else
   v := E[1];
  fi;
 fi;

 w := L[v][-1];
 t := [v,w];
 xx := {op(L[w])} minus {op(t)};
 while xx <> {} do
  w := xx[1];
  t := [op(t),w];
  xx := {op(L[w])} minus {op(t)};
 od;

 if E = [] then t := [op(t),t[1]]; fi;

 return t;
end:

`barycentric_subdivision/raw_simplicial_complex` := proc(K)
 local V,n,i,j,P,L,d,s,t,p;

 V := `all_simplices/raw_simplicial_complex`(K);

 n := max(map(nops,V));
 for i from 1 to n do
  P[i] := combinat[permute](i);
 od:

 L := NULL;
 for s in K do
  d := nops(s);
  for p in P[d] do
   t := {seq(sort([seq(s[p[i]],i=1..j)]),j=1..d)};
   L := L,t
  od;
 od;

 return({L});
end:

`barycentric_subdivision/simplicial_complex` := proc(T)
 local K,E,K0,V0,T0,E0,d,s,x,v;

 K := T["max_simplices"];
 K0 := `barycentric_subdivision/raw_simplicial_complex`(K);
 V0 := `vertices/raw_simplicial_complex`(K0);
 T0 := table():
 T0["vertices"] := V0;
 T0["max_simplices"] := K0;

 E := T["embedding"];
 if type(E,table) then
  d := T["embedding_dim"];
  T0["embedding_dim"] := d;
  E0 := table();
  for s in V0 do
   x := [0$d];
   for v in s do x := x +~ E[v]; od;
   x := x /~ nops(s);
   E0[s] := x;
  od;
  T0["embedding"] := eval(E0);
 fi;

 return eval(T0);
end:

`triangular_subdivision/simplicial_complex` := proc(T)
 local V,E,F,V0,E0,E1,E2,F0,T0,v,e,f,d,P,P0;

 if `dim/simplicial_complex`(T) > 2 then
  error "Triangular subdivision is only for complexes of dimension <= 2";
  # There is a version for dimension 3, with an extra simplex at the
  # barycentre of each tetrahedron as well as each edge, but we have
  # not coded it yet.  Perhaps there is a version where we adjoin the
  # barycentre of each simplex of odd dimension?  Note that even in
  # dimension two, this triangular subdivision is different from, and
  # more symmetrical than, the edgewise subdivision.  The edgewise
  # subdivision needs some kind of order on the vertices.
 fi;
 V := T["vertices"];
 E := `edges/raw_simplicial_complex`(T["max_simplices"]);
 F := `faces/raw_simplicial_complex`(T["max_simplices"]);
 E := map(e -> sort([op(e)]),E);
 E := sort([op(E)]);
 F := map(f -> sort([op(f)]),F);
 F := sort([op(F)]);

 V0 := [seq([v],v in V),seq(sort([op(e)]),e in E)];
 E0 := [seq(op([[[e[1]],e],[[e[2]],e]]), e in E),
        seq(op([
	 [[f[1],f[2]],[f[1],f[3]]],
	 [[f[1],f[2]],[f[2],f[3]]],
	 [[f[1],f[3]],[f[2],f[3]]]
	]), f in F)
       ];
 F0 := [seq(op([
         [[f[1]],[f[1],f[2]],[f[1],f[3]]],
         [[f[2]],[f[1],f[2]],[f[2],f[3]]],
         [[f[3]],[f[1],f[3]],[f[2],f[3]]],
	 [[f[1],f[2]],[f[1],f[3]],[f[2],f[3]]]
        ]),f in F)];

 T0 := table():
 T0["vertices"] := V0;
 T0["edges"] := E0;
 T0["faces"] := F0;

 E1 := {op(map(f -> op({[f[1],f[2]],[f[1],f[3]],[f[2],f[3]]}),F0))};
 E2 := {op(E0)} minus E1;
 E2 := [op(E2)];
 T0["max_simplices"] := [op(E2),op(F0)];

 P := T["embedding"];
 if type(P,table) then
  d := T["embedding_dim"];
  T0["embedding_dim"] := d;
  P0 := table();
  for v in V do
   P0[[v]] := P[v];
  od:

  for e in E do
   P0[e] := (P[e[1]] +~ P[e[2]]) /~ 2;
  od:
  T0["embedding"] := eval(P0);
 fi;

 return T0;
end:

`partial_triangular_subdivision/simplicial_complex` := proc(T,S)
 local S0,Si,V,E,F,V0,E0,F0,nV,nS,e,e1,e2,e3,f,is_long,fin,
  long_edges,p,p1,p2,p3,q,i,T0;

 S0 := map(e -> sort([op(e)]),S);
 S0 := sort([op(S0)]);

 E := `edges/raw_simplicial_complex`(T["max_simplices"]);
 F := `faces/raw_simplicial_complex`(T["max_simplices"]);
 E := map(e -> sort([op(e)]),E);
 E := sort([op(E)]);
 F := map(f -> sort([op(f)]),F);
 F := sort([op(F)]);

 is_long := table():
 for e in E do
  is_long[e] := false;
 od:
 for e in S0 do
  is_long[e] := true;
 od:

 fin := false;
 long_edges := table():

 while not fin do
  fin := true;

  for f in F do
   e1 := [f[1],f[2]];
   e2 := [f[1],f[3]];
   e3 := [f[2],f[3]];
   long_edges[f] := select(e -> is_long[e],[e1,e2,e3]);
   if nops(long_edges[f]) = 2 then
    fin := false;
    is_long[e1] := true;
    is_long[e2] := true;
    is_long[e3] := true;
   fi;
  od:
 od:

 S0 := select(e -> is_long[e],E);
 V := T["vertices"];
 nV := nops(V):
 nS := nops(S0):

 Si := table():
 for i from 1 to nS do
  Si[S0[i]] := nV + i;
 od:

 F0 := NULL;
 for f in F do
  e1 := [f[1],f[2]];
  e2 := [f[1],f[3]];
  e3 := [f[2],f[3]];
  if nops(long_edges[f]) = 0 then
  if nops(f) < 3 then print(["err0",f]); fi;
   F0 := F0,f;
  elif nops(long_edges[f]) = 1 then
   e := long_edges[f][1];
   p := ({op(f)} minus {op(e)})[1];
   q := Si[long_edges[f][1]];
   if nops({e[1],p,q}) < 3 then print(["err1",f,e[1],p,q]); fi;
   if nops({e[2],p,q}) < 3 then print(["err2",f,e[2],p,q]); fi;
   F0 := F0,[e[1],p,q],[e[2],p,q];
  elif nops(long_edges[f]) = 2 then
   print(["shouldn't happen",f]);
  else
   p1 := Si[e1]; p2 := Si[e2]; p3 := Si[e3];
   if nops({f[1],p1,p2}) < 3 then print(["err3",f,p1,p2,p3]); fi;
   if nops({f[2],p1,p3}) < 3 then print(["err4",f,p1,p2,p3]); fi;
   if nops({f[3],p2,p3}) < 3 then print(["err4",f,p1,p2,p3]); fi;
   if nops({p1,p2,p3})   < 3 then print(["err5",p1,p2,p3]); fi;
   F0 := F0, [f[1],p1,p2],[f[2],p1,p3],[f[3],p2,p3],[p1,p2,p3];
  fi;
 od:
 V0 := [seq(i,i=1..nV+nS)];
 F0 := sort([op(map(sort,{F0}))]);
 T0 := table():
 T0["vertices"] := V0;
 T0["max_simplices"] := F0;
 T0["embedding_dim"] := T["embedding_dim"]:
 T0["embedding"] := table():
 for i from 1 to nV do T0["embedding"][i] := T["embedding"][i]; od;
 for i from 1 to nS do
  e := S0[i];
  T0["embedding"][nV+i] := 0.5 *~ (T["embedding"][e[1]] +~ T["embedding"][e[2]]);
 od;
 `set_faces/simplicial_complex`(T0,true);
 `set_edges/simplicial_complex`(T0,true);

 return eval(T0);
end:

`star_subdivide/simplicial_complex` := proc(T,s)
 local n,V,K,K1,S0,S1,S2,s0,s1,s2,u,P,d,x,v,i;

 if nops(s) = 1 then return; fi;

 n := nops(T["vertices"]);
 V := [seq(i,i=1..n)];
 if {op(T["vertices"])} <> {op(V)} then
  error "Star subdivision is only implemented for condensed complexes";
 fi;

 K := T["max_simplices"];
 K := map(u -> {op(u)},{op(K)});

 s0 := {op(s)};
 S1,S0 := selectremove(u -> s0 minus {op(u)} = {},K);
 S2 := {seq(seq(u minus {v},v in s),u in S1)};

 K1 := {op(S0),seq({op(u),n+1},u in S2)};
 K1 := map(u -> sort([op(u)]),K1);
 K1 := sort([op(K1)]);

 T["vertices"] := [seq(i,i=1..n+1)];
 T["max_simplices"] := K1;

 P := T["embedding"];
 if type(P,table) then
  d := T["embedding_dim"];
  x := [0$d];
  for v in s do x := x +~ P[v]; od;
  x := evalf(x /~ nops(s));
  P[n+1] := x;
 fi;
end:

# This function can be applied to a simplicial surface T with a
# vertex v whose link is an octagon, thought of as a bisected
# square.  The vertex w is the midpoint of one edge of the square.
# This function performs a subdivision that affects the star of v
# by adding four new vertices at the midpoints of the edges
# joining v to the corners of the square.  It returns a new vertex
# w', so that the process can be repeated using v and w'.
`square_refine/simplicial_complex` := proc(T,v,w)
 local n,K,L,W,P,old_faces,new_faces,i;
 if not(`is_condensed/simplicial_complex`(T)) then
  error "Complex is not condensed";
 fi;

 n := nops(T["vertices"]);
 L := `link/simplicial_complex`(T,v);
 W := `track/simplicial_complex`(L,w);

 if nops(W) <> 9 then
  error "Link is not square";
 fi;

 old_faces := {seq({v,W[i],W[i+1]},i=1..8)};
 new_faces := {
  {v,W[1],n+1},{v,n+1,W[3]},{v,W[3],n+2},{v,n+2,W[5]},
  {v,W[5],n+3},{v,n+3,W[7]},{v,W[7],n+4},{v,n+4,W[1]},
  {W[1],W[2],n+1},{W[2],W[3],n+1},{W[3],W[4],n+2},{W[4],W[5],n+2},
  {W[5],W[6],n+3},{W[6],W[7],n+3},{W[7],W[8],n+4},{W[8],W[1],n+4}
 };

 T["vertices"] := [seq(i,i=1..n+4)];
 K := T["max_simplices"];
 K := {op(map(s -> {op(s)},K))};
 K := (K minus old_faces) union new_faces;
 K := map(s -> sort([op(s)]),K);
 K := sort([op(K)]);
 T["max_simplices"] := K;

 if `has_embedding/simplicial_complex`(T) then
  P := T["embedding"];
  P[n+1] := (P[v] +~ P[W[2]]) /~ 2;
  P[n+2] := (P[v] +~ P[W[4]]) /~ 2;
  P[n+3] := (P[v] +~ P[W[6]]) /~ 2;
  P[n+4] := (P[v] +~ P[W[8]]) /~ 2;
 fi;

 return n+1;
end:

# Here P is expected to be a table indexed by points
# i in the 2-simplex for which 2^m * i is integral.
# The values are either NULL or vectors in R^3, and
# the values at the vertices of the 2-simplex must
# be in R^3.  The third parameter r is supposed to
# be a retraction of R^3 onto a subspace Z, and it
# is expected that the non-NULL values of P should
# lie in that subspace.  The procedure fills in the
# values P[i] for all i so they lie in Z and are
# spaced out in a reasonably regular way.

`midpoint_extend/simplicial_complex` := proc(P,m,r)
 local X0,p,q,i,j,k,x,y,u,N;

 X0 := {[1,-1,0],[-1,1,0],[1,0,-1],[-1,0,1],[0,1,-1],[0,-1,1]};

 for p from 0 to m do
  q := 2^p;
  for i from 0 to q do
   for j from 0 to q - i do
    k := q - i - j;
    x := [i,j,k];
    if P[x /~ q] = NULL then
     N := select(y -> min(op(x +~ y)) >= 0 and
                      min(op(x -~ y)) >= 0 and
                      P[(x +~ y) /~ q] <> NULL and
                      P[(x -~ y) /~ q] <> NULL,X0):
     if N = {} then DEBUG(); fi;
     u := [0,0,0]:
     for y in N do u := u +~ P[(x +~ y)/~ q]; od:
     u := evalf(u /~ nops(N));
     u := r(u);
     P[x /~ q] := u;
    fi:
   od:
  od:
 od:
end:

`normalise_embedding/simplicial_complex` := proc(T)
 local d,P,v,x,i;

 d := T["embedding_dim"];
 P := T["embedding"];
 for v in T["vertices"] do
  x := evalf(P[v]);
  if not(type(x,[numeric $d])) then
   print([v,x]);
   error "Bad vertex position";
  fi;
  x := evalf(x /~ sqrt(add(x[i]^2,i=1..d)));
  P[v] := x;
 od:
end:

`map_in_place/simplicial_complex` := proc(f,T)
 local E,v;

 E := eval(T["embedding"]);
 for v in T["vertices"] do E[v] := f(E[v]); od:
end:

`map/simplicial_complex` := proc(f,T)
 local T0;

 T0 := eval(`clone/simplicial_complex`(T));
 `map_in_place/simplicial_complex`(f,T0);
 return eval(T0);
end:

`plot/raw_simplicial_complex` := proc(K,d,a)
 local V,E,F,v,e,f,P;

 V := `vertices/raw_simplicial_complex`(K);
 E := `edges/raw_simplicial_complex`(K);
 F := `faces/raw_simplicial_complex`(K);

 P :=
  seq(point(a[v]),v in V),
  seq(line(a[e[1]],a[e[2]]),e in E),
  seq(polygon([a[f[1]],a[f[2]],a[f[3]]],colour=red),f in F);

 return display(P,scaling=constrained,axes=none):
end:

`plot/simplicial_complex` := proc(T,a_)
 local V,E,F,v,e,f,a;

 `set_edges/simplicial_complex`(T);
 `set_faces/simplicial_complex`(T);
 V := T["vertices"];
 E := T["edges"];
 F := T["faces"];
 a := `if`(nargs > 1, a_, T["embedding"]);
 T["plot"] := display(
  seq(point(a[v]),v in V),
  seq(line(a[e[1]],a[e[2]]),e in E),
  seq(polygon([a[f[1]],a[f[2]],a[f[3]]],colour=red),f in F),
  scaling=constrained,axes=none):
 return T["plot"];
end:

`surface_plot/simplicial_complex` := proc(T,a_)
 local V,E,F,f,a;

 `set_faces/simplicial_complex`(T);
 F := T["faces"];
 a := `if`(nargs > 1, a_, T["embedding"]);
 T["surface_plot"] := display(
  seq(polygon([a[f[1]],a[f[2]],a[f[3]]],style=patchnogrid),f in F),
  scaling=constrained,axes=none):
 return T["surface_plot"];
end:

`vertex_labels/simplicial_complex` := proc(T)
 local v;
 if T["embedding_dim"] = 2 then
  display(seq(textplot([op(T["embedding"][v]),sprintf("%A",v)]),v in T["vertices"]),
          axes=none,scaling=constrained);
 elif T["embedding_dim"] = 3 then
  display(seq(textplot3d([op(T["embedding"][v]),sprintf("%A",v)]),v in T["vertices"]),
          axes=none,scaling=constrained);
 else
  error "No 2 or 3 dimensional embedding";
 fi;
end:

`set_javascript/simplicial_complex` := proc(T,format_)
 local format,format3,V,F,F1,P,N,V_string,N_string,F_string,T_string,f,f1,n1,n2,v,i;

 format := `if`(nargs > 1,format_,"%f");
 format3 := sprintf("[%s,%s,%s]",format,format,format);
 if not(`has_embedding/simplicial_complex`(T)) then
  error "Complex has no embedding data";
 fi;

 if T["embedding_dim"] <> 3 then
  error "Complex is not in R3";
 fi;

 V := T["vertices"];
 P := eval(T["embedding"]);
 `set_faces/simplicial_complex`(T);
 F := T["faces"];
 F := sort([op(map(f -> sort([op(f)]),F))]);

 V_string :=
  cat("[",StringTools[Join]([seq(sprintf(format3,op(evalf(P[v]))),v in V)],","),"]");

 if type(T["normal"],table) then
  N := eval(T["normal"]);
  N_string :=
   cat("[",StringTools[Join]([seq(sprintf(format3,op(evalf(N[v]))),v in V)],","),"]");
  F1 := NULL;
  for f in F do
   n1 := cross_product(P[f[2]] -~ P[f[1]],P[f[3]] -~ P[f[1]]);
   n2 := N[f[1]] +~ N[f[2]] +~ N[f[3]];
   if add(n1[i] * n2[i],i=1..3) > 0 then
    f1 := [f[1]-1,f[2]-1,f[3]-1];
   else
    f1 := [f[1]-1,f[3]-1,f[2]-1];
   fi;
   F1 := F1,f1;
  od:
  F1 := [F1];
  F_string :=
   cat("[",StringTools[Join]([seq(sprintf("%A",f),f in F1)],","),"]");
  T_string :=
   sprintf("{\npositions:\n%s,\nnormals:\n%s,indices:\n%s }",
           V_string,N_string,F_string);
 else
  F_string :=
   cat("[",StringTools[Join]([seq(sprintf("%A",f),f in F)],","),"]");
  T_string :=
   sprintf("{\npositions:\n%s,indices:\n%s }",
           V_string,F_string);
 fi;

 T["javascript"] := T_string;

 return T_string;
end:


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

`is_element/WFbar` := (N::posint) -> (A::set) -> proc(TTxl)
 local TT,x,l,TT1,T,n,u,children;
 global reason;

 if not(type(TTxl,list) and nops(TTxl) = 3) then
  reason := [convert(procname,string),"TTxl cannot be split as [TT,x,l]",TTxl];
  return false;
 fi;

 TT,x,l := op(TTxl);

 if not(`is_element/trees`(A)(TT)) then
  reason := [convert(procname,string),"TT is not a tree on A",TT,A];
  return false;
 fi;

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

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

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

 children := `children_map`(A)(TT);
 for T in TT1 do
  if not(`is_element/Fbar`(N)(children[T])(x[T])) then
   reason := [convert(procname,string),"x[T] has invalid type",T,x[T],reason];
   return false;
  fi;
 od;

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

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

 n := nops(A);

 for T in TT do
  if not(`is_element/RR`(l[T]) and l[T] >= 0 and l[T] <= 1) then
   reason := [convert(procname,string),"l[T] is not in the unit interval",T,l[T]];
   return false;
  fi;

  if nops(T) = 1 and l[T] <> 1 then
   reason := [convert(procname,string),"l[{a}] <> 1",op(T),l[T]];
   return false;
  fi;

  if nops(T) = n and l[T] <> 1 then
   reason := [convert(procname,string),"l[A] <> 1",T,l[T]];
   return false;
  fi;
 od;

 return true;
end;

`is_interior/WFbar` := (N::posint) -> (A::set) -> proc(TTxl)
 local TT,x,l,n,T;
 global reason;

 TT,x,l := op(TTxl);
 n := nops(A);

 for T in TT do
  if nops(T) > 1 and nops(T) < n and l[T] = 1 then
   reason := [convert(procname,string),"not interior, l[T] = 1",T];
   return false;
  fi;
 od:

 return true;
end;

`is_reduced/WFbar` := (N::posint) -> (A::set) -> proc(TTxl)
 local TT,x,l,T;
 global reason;

 TT,x,l := op(TTxl);

 for T in TT do
  if l[T] = 0 then
   reason := [convert(procname,string),"not reduced, l[T] = 0",T];
   return false;
  fi;
 od:

 return true;
end;

`reduce_once/WFbar` := (N::posint) -> (A::set) -> proc(TTxl,T)
 local TT,x,l,parent,children,n,TT1,x1,l1,U,B,C,X,p,y,z;

 TT,x,l := op(TTxl);

 parent := `parent_map`(A)(TT);
 children := `children_map`(A)(TT);

 TT1 := TT minus {T};

 U := parent[T];
 C := children[U];
 B := C minus {T} union children[T];

 p := table();
 for X in C minus {T} do p[X] := X; od;
 for X in children[T] do p[X] := T; od;

 y := table();
 for X in C minus {T} do y[X] := `eta/Fbar`(N)({X}); od;
 y[T] := x[T];

 z := `gamma/Fbar`(N)(B,C)(p)(x[U],y);

 x1 := table();
 l1 := table();

 for X in TT1 do
  l1[X] := l[X];
  if nops(X) > 1 then
   x1[X] := `if`(X = U,eval(z),eval(x[X]));
  fi;
 od;

 return([TT1,eval(x1),eval(l1)]);
end;

`reduce/WFbar` := (N::posint) -> (A::set) -> proc(TTxl)
 local is_reduced,X,TT,x,l,T;

 is_reduced := false;
 X := TTxl;

 while not(is_reduced) do
  TT,x,l := op(X);
  is_reduced := true;

  for T in TT do
   if l[T] = 0 then
    is_reduced := false;
    X := `reduce_once/WFbar`(N)(A)(X,T);
    break;
   fi;
  od:
 od;

 return X;
end;

`is_equal/WFbar` := (N::posint) -> (A::set) -> proc(X0,X1)
 local Y0,Y1,TT0,TT1,x0,x1,l0,l1,children,T;
 global reason;

 Y0 := `reduce/WFbar`(N)(A)(X0);
 Y1 := `reduce/WFbar`(N)(A)(X1);
 TT0,x0,l0 := op(Y0);
 TT1,x1,l1 := op(Y1);

 if TT0 <> TT1 then
  reason := [convert(procname,string),"TT0 <> TT1",TT0,TT1];
  return false;
 fi;

 children := `children_map`(A)(TT0);

 for T in TT0 do
  if l0[T] <> l1[T] then
   reason := [convert(procname,string),"l0[T] <> l1[T]",T,l0[T],l1[T]];
   return false;
  fi;

  if nops(T) > 1 then
   if not(`is_equal/Fbar`(N)(children[T])(x0[T],x1[T])) then
    reason := [convert(procname,string),"x0[T] <> x1[T]",T,x0[T],x1[T],reason];
    return false;
   fi;
  fi;
 od;

 return true;
end;

`is_leq/WFbar` := NULL;

`random_element/WFbar` := (N::posint) -> (A::set) -> proc()
 local TT,x,l,TT1,T,n,children,d;

 d := 3;
 TT := `random_element/full_trees`(A)();

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

 x := table();
 l := table();

 children := `children_map`(A)(TT);
 for T in TT1 do
  x[T] := `random_element/Fbar`(N)(children[T])();
 od;

 n := nops(A);

 for T in TT do
  if nops(T) = 1 or nops(T) = n then
   l[T] := 1;
  else
   l[T] := rand(0..d)()/d;
  fi;
 od;

 return [TT,eval(x),eval(l)];
end;

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


# File: RigidMotionsRecoverNMM.mpl
#
# Description:
#  This file contains functions used to obtain an arrangement 6 dimensional parameter space of 3D
#  digitized rigid motions.
#  This code has been written for research propose and its aim is to calculate a particular
#  arrangement of quadrics. Therefore, it can or it cannot be useful in study of generic
#  arrangements. The final output are sample points of full dimensional open cells.
#
#  The code was written in relation with the paper: Kacper Pluta, Guillaume Moroz, Yukiko
#  Kenmochi, Pascal Romon, Quadric arrangement in classifying rigid motions of a 3D digital image,
#  2016, https://hal.archives-ouvertes.fr/hal-01334257 referred late on as [Quadrics:2016].
#
# Author:
#  Kacper Pluta - kacper.pluta@esiee.fr
#  Laboratoire d'Informatique Gaspard-Monge - LIGM, A3SI, France
#
# Date:
#  11/12/2015
#
# License:
#  Simplified BSD License
#
# Copyright (c) 2015, Kacper Pluta
# All rights reserved.

# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#   * Redistributions of source code must retain the above copyright
#     notice, this list of conditions and the following disclaimer.
#   * Redistributions in binary form must reproduce the above copyright
#     notice, this list of conditions and the following disclaimer in the
#     documentation and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL Kacper Pluta BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
#

RigidMotionsRecoverNMM := module()
  option package;
  uses   RigidMotionsParameterSpaceCommon, RigidMotionsMaplePrimesCode;

  (* String which represents a type of the neighborhood i.e. N1, N2 or N3. *)
  global nTypeGlobal;
  (* String which represents the path to the database. *)
  global dbPathGlobal;
  (* List of integers which are the indices of half-grid planes. *)
  global kRangeGlobal;
  (* List of the variables in which the problem is expressed. *)
  global varsGlobal;

  (*Controls how many sample points should be fetch from the database in one quary.*)
  export BUFFER_SIZE := 1000;

  export ParallelCalculateNMM, Get3DNMM, RecoverTranslationSamplePoints, GetOrderedCriticalPlanes,
  CriticalPlanes, CalculateNMM, LaunchFindDistinctSamplePoints,  LaunchComputeNMM,
  FetchSamplePointsFromDB, FetchTopologicallyDistinctSamplePointsFromDB,
  ParallelFindTopologicallyDistinctSamplePoints;

# Procedure: CriticalPlanes
#   Compute critical planes in the remainder range
#
# Parameters:
#   R                - the rotation matrix obtained from CayleyTransform
#   neighborhood     - a neighborhood for which one wants to compute NMM
#   kRange           - a range of planes to consider
#
# Output :
#   Returns a list of lists each containing critical planes for one direction
CriticalPlanes := proc(R::Matrix, neighborhood::list, kRange::list)
  # we remove the element [0, 0, 0]
  local n := subsop(ListTools:-Search([0,0,0], neighborhood)=NULL, neighborhood);
  local T := combinat:-cartprod([n, kRange]);
  local planes := [[],[],[]], params;
  while not T[finished] do
    params := T[nextvalue]();
    planes[1] := [op(planes[1]), (R . Vector(3, params[1]) - Vector(3, params[2]-1/2))[1]];
    planes[2] := [op(planes[2]), (R . Vector(3, params[1]) - Vector(3, params[2]-1/2))[2]];
    planes[3] := [op(planes[3]), (R . Vector(3, params[1]) - Vector(3, params[2]-1/2))[3]];
  end do;
 return planes;
end proc;

# Procedure: Get3DNMM
#   Compute neighbourhood motion maps
#
# Parameters:
#   neighborhood     - a neighborhood for which one wants to compute NMM
#   sampleTrans     - a midpoint of a frame in the remainder range
#   R                - the rotation matrix obtained from CayleyTransform
#
# Output:
#   Returns 3D neighborhood motion map for given vars and corresponding translations.
Get3DNMM := proc(neighborhood::list, sampleTrans::Vector, R::Matrix)
  local n, NMM := [];
  for n in neighborhood do
    NMM := [op(NMM), convert(map[inplace](round, R.Vector(3, n) + sampleTrans), list)];
  od;
  return NMM;
end proc;

# Procedure: RecoverTranslationSamplePoints
#   Compute midpoints of each frame in the remainder range
#
# Parameters:
#   planes            - ordered list of critical planes in the remainder range
#
# Output:
#   Returns centers of frames in the remainder range
RecoverTranslationSamplePoints := proc(planes::list)
  local s:
    s := proc(planes::list, i::integer, j::integer, k::integer)
      return [(1/2) * add(planes[1][i .. i+1]), (1/2)*add(planes[2][j.. j+1]),
                                            (1/2)*add(planes[3][k .. k+1])];
  end proc;
  return [seq(seq(seq(s(planes, i, j, k), k=1..nops(planes[3]-1)),
               j=1..nops(planes[2]-1)), i =1..nops(planes[1])-1)];
end proc;

# Procedure: GetOrderedCriticalPlanes
#   Compute critical planes in the remainder range
#
# Parameters:
#   vars             - a list of variables
#   samplePoint      - a rotational sample point
#   planes           - a precomputed list of planes in the remainder range
#
# Output:
#   Returns the ordered critical planes in the remainder range for X, Y and Z directions and order
#   signature.
GetOrderedCriticalPlanes := proc(vars::list, samplePoint::list, planes::list)
  local params, sdPlanes := [[],[],[]];
  local xSig, ySig, zSig, Signature;

  if nops(vars) <> 3 then
    error "Only 3D arrangement is supported.";
  fi;

  sdPlanes[1] := eval(planes[1], [vars[1] = samplePoint[1], vars[2] = samplePoint[2], vars[3] =
                                                                                samplePoint[3]]);
  sdPlanes[2] := eval(planes[2], [vars[1] = samplePoint[1], vars[2] = samplePoint[2], vars[3] =
                                                                                samplePoint[3]]);
  sdPlanes[3] := eval(planes[3], [vars[1] = samplePoint[1], vars[2] = samplePoint[2], vars[3] =
                                                                                samplePoint[3]]);

  xSig, sdPlanes[1] := Isort(sdPlanes[1]);
  ySig, sdPlanes[2] := Isort(sdPlanes[2]);
  zSig, sdPlanes[3] := Isort(sdPlanes[3]);

  # remove all out of the range regions
  sdPlanes[1] := remove(proc(x) return evalb(x <= -1/2) end proc,
  remove(proc(x) return evalb(x >= 1/2) end proc, sdPlanes[1]));
  sdPlanes[2] := remove(proc(x) return evalb(x <= -1/2) end proc,
  remove(proc(x) return evalb(x >= 1/2) end proc, sdPlanes[2]));
  sdPlanes[3] := remove(proc(x) return evalb(x <= -1/2) end proc,
  remove(proc(x) return evalb(x >= 1/2) end proc, sdPlanes[3]));
  # add region boarders
  sdPlanes[1] := [-1/2,op(sdPlanes[1]),1/2];
  sdPlanes[2] := [-1/2,op(sdPlanes[2]),1/2];
  sdPlanes[3] := [-1/2,op(sdPlanes[3]),1/2];

  Signature := cat(op(map(proc (x) sprintf("%d", x) end proc, [op(xSig), op(ySig), op(zSig)])));
  return Signature, sdPlanes;
end proc:

# Procedure: CalculateNMM
#   Reads data from hard drive and generates NMM
#
# Parameters:
#   vars             - list of variables in which the problem is expressed
#   planes           - a precomputed list of planes in the remainder range
#   buffer           - an Array which contains rotational sample points
#   N                - a given neighborhood
#   R                - a Matrix computed with Cayley transform
#   db               - an instance of ComputationRegister with open connection to the database
# Output:
#   A neighborhood motion map is saved in the database.
#
CalculateNMM := proc(vars::list, planes::list, buffer::Array, N::list, R::Matrix,
                                                         db::ComputationRegister)
  local sdPlanes, trans, x, RR, y, NMM;
  for x in buffer do
    RR := eval(R, [vars[1] = x[2], vars[2] = x[3], vars[3] = x[4]]);
    sdPlanes := GetOrderedCriticalPlanes(vars, x[2..()], planes)[2];
    trans := RecoverTranslationSamplePoints(sdPlanes);
    for y in trans do
      NMM := Get3DNMM(N, Vector(3, y), RR);
      InsertNMM(db, x[1], NMM, y);
    od;
  end do;
end proc:

# Procedure: FetchTopologicallyDistinctSamplePointsFromDB
#   Used to fetch topologically distinct rotational sample points from the database. The number of
#   sample points is controlled by BUFFER_SIZE.
#
# Parameters:
#   db::ComputationRegister       - an instance of ComputationRegister with open connection to the
#                                   database.
#   fromID::integer               - an id of the first sample point from a range to be fetched
#   last::integer                 - an id of the very last sample points to be fetched
#
# Output:
#   An Array of lists which of each represent a sample point. Note that the first element of each
#   list is an id of a given sample point.
FetchTopologicallyDistinctSamplePointsFromDB := proc(db::ComputationRegister, fromID::integer,
                                                                                last::integer)
  local n := fromID + RigidMotionsRecoverNMM:-BUFFER_SIZE - 1;
  if n > last then
    n := last;
  fi;
  return FetchTopologicallyDistinctSamplePoints(db, fromID, n);
end proc;

# Procedure: ParallelCalculateNMM
#   Uses Grid framework to generates unique NMM.
#
ParallelCalculateNMM := proc()
  local db:= Object(ComputationRegister, dbPathGlobal), n;
  local noTPoints;
  local first::integer, last::integer;
  local R := CayleyTransform(varsGlobal), N := GetNeighborhood(nTypeGlobal);
  local planes := RigidMotionsRecoverNMM:-CriticalPlanes(R, N, kRangeGlobal);
  local i::integer, buffer, samplePoint, sig::string;
  noTPoints := NumberOfTopologicallyDistinctSamplePoints(db);

  n := trunc(noTPoints / Grid:-NumNodes());
  first :=  Grid:-MyNode() * n + 1; last := (Grid:-MyNode() + 1) * n;
  if Grid:-MyNode() = Grid:-NumNodes() - 1 then
    last := noTPoints;
  fi;

  for i from first by RigidMotionsRecoverNMM:-BUFFER_SIZE to last do
    buffer := RigidMotionsRecoverNMM:-FetchTopologicallyDistinctSamplePointsFromDB(db, i, last);
    RigidMotionsRecoverNMM:-CalculateNMM(varsGlobal, planes, buffer, N, R, db);
    SynchronizeNMM(db);
  od;
  Close(db);
  Grid:-Barrier();
end proc:

# Procedure: FetchSamplePointsFromDB
#   Used to fetch rotational sample points from the database. The number of sample points is
#   controlled by BUFFER_SIZE.
#
# Parameters:
#   db::ComputationRegister       - an instance of ComputationRegister with open connection to the
#                                   database.
#   fromID::integer               - an id of the first sample point from a range to be fetched
#   last::integer                 - an id of the very last sample points to be fetched
#
# Output:
#   An Array of lists which of each represent a sample point. Note that the first element of each
#   list is an id of a given sample point.
FetchSamplePointsFromDB := proc(db::ComputationRegister, fromID::integer, last::integer)
  local n := fromID + RigidMotionsRecoverNMM:-BUFFER_SIZE - 1;
  if n > last then
    n := last;
  fi;
  return FetchSamplePointsWithoutSignature(db, fromID, n );
end proc;

# Procedure: ParallelFindTopologicallyDistinctSamplePoints
#   Finds rotational sample points which lead to unique arrangement of the critical planes in the
#   remainder range.
#
 ParallelFindTopologicallyDistinctSamplePoints := proc()
  local first::integer, last::integer;
  local R := CayleyTransform(varsGlobal), N := GetNeighborhood(nTypeGlobal);
  local planes := RigidMotionsRecoverNMM:-CriticalPlanes(R, N, kRangeGlobal);
  local db, n, i::integer, buffer, samplePoint, sig, noTPoints;
  db:= Object(ComputationRegister, dbPathGlobal);
  noTPoints := NumberOfSamplePoints(db);
  n := trunc(noTPoints / Grid:-NumNodes());
  first :=  Grid:-MyNode() * n + 1; last := (Grid:-MyNode() + 1) * n;
  if Grid:-MyNode() = Grid:-NumNodes() - 1 then
    last := noTPoints;
  fi;

  for i from first by RigidMotionsRecoverNMM:-BUFFER_SIZE to last do
    buffer := RigidMotionsRecoverNMM:-FetchSamplePointsFromDB(db, i, last);
    for samplePoint in buffer do
      sig := RigidMotionsRecoverNMM:-GetOrderedCriticalPlanes(varsGlobal,samplePoint[2..()],planes)[1];
      InsertSignature(db, samplePoint[1], sig);
    od;
  od;
  SynchronizeSamplePointsSignatures(db);
  Close(db);
  Grid:-Barrier();
end proc;

LaunchFindDistinctSamplePoints := proc(vars::list, nType::string, kRange::list, dbPath::string,
                                                        nodes:=kernelopts(numcpus))
  local db:=Object(ComputationRegister, dbPath);
  PrepareSamplePoints(db);
  Close(db);
  nTypeGlobal := nType; kRangeGlobal := kRange; dbPathGlobal := dbPath; varsGlobal := vars;

  Grid:-Setup("local");
  Grid:-Launch(RigidMotionsRecoverNMM:-ParallelFindTopologicallyDistinctSamplePoints,
  imports=['varsGlobal', 'nTypeGlobal', 'kRangeGlobal', 'dbPathGlobal'], numnodes=nodes,
  allexternal=false);
  db:=Object(ComputationRegister, dbPath);
  CloseSignaturesAddition(db);
  Close(db);
end proc:

# Procedure: LaunchOnGridGetNMM
#   Setup and run computation on a local grid
#
# Parameters:
#   vars             - list of variables in which the problem is expressed
#   nType            - size of neighborhood i.e N1, N2 and N3.
#   kRange           - a range of planes of the half-grid
#   dbPath           - a path to a database file.
#   nodes            - number of nodes used in the parallel computations
#
# Output:
#   List of unique neighborhood motion maps
LaunchComputeNMM := proc(vars::list, nType::string, kRange::list, dbPath::string,
                                                        nodes:=kernelopts(numcpus))
  local db:=Object(ComputationRegister, dbPath);
  DropRedundantSamplePoints(db);
  Close(db);
  nTypeGlobal := nType; kRangeGlobal := kRange; dbPathGlobal := dbPath; varsGlobal := vars;

  Grid:-Setup("local");
  Grid:-Launch(RigidMotionsRecoverNMM:-ParallelCalculateNMM, imports=['varsGlobal', 'nTypeGlobal',
  'kRangeGlobal', 'dbPathGlobal'], numnodes=nodes, allexternal=false);
end proc:

end module:


#
#  Currently, these are not 'tests' in the usual sense, in that they do
# not have correct answers encoded.  They are just meant to all be
# runnable (and are part of the stuff serving as input for the array
# simplification paper we're writing).

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(NewSLO):
with(Loop):

kb := KB:-assert(n :: nonnegint, KB:-empty):
i, kb := KB:-genType(i, HInt(closed_bounds(1..n)), kb):

t1 := Product(idx(x,j),j=1..n):
r1 := unproduct(t1, x, i=1..n, [], `*`, kb, kb);

t2 := 2:
r2 := unproduct(t2, x, i=1..n, [], `*`, kb, kb);

t3 := 2^n:
r3 := unproduct(t3, x, i=1..n, [], `*`, kb, kb);

t4 := Product(2*idx(x,j),j=1..n);
r4 := unproduct(t4, x, i=1..n, [], `*`, kb, kb);

t5 := Product(2+idx(x,j),j=1..n);
r5 := unproduct(t5, x, i=1..n, [], `*`, kb, kb);

t6 := Product(f(idx(x,j)),j=1..n);
r6 := unproduct(t6, x, i=1..n, [], `*`, kb, kb);

t7 := exp(Sum(g(idx(x,k)),k=1..n));
r7 := unproduct(t7, x, i=1..n, [], `*`, kb, kb);

t8 := t6*t7;
r8 := unproduct(t8, x, i=1..n, [], `*`, kb, kb);

t9 := Product(f(idx(x,j+1)),j=0..n-1);
r9 := unproduct(t9, x, i=1..n, [], `*`, kb, kb);

t10 := Product(f(idx(x,j)),j=1..n-1);
r10 := unproduct(t10, x, i=1..n, [], `*`, kb, kb);

# oops, this ones doesn't !!  Going under (above) is
# ok, but over does not.  Neither of the following:
# t10a := Product(f(idx(x,j)),j=1..n+1);
# r10a := unproduct(t10, x, i=1..n, [], `*`, kb, kb);
# t10b := Product(f(idx(x,j)),j=0..n);
# r10b := unproduct(t10, x, i=1..n, [], `*`, kb, kb);

t11 := Product(exp(Sum(f(idx(x,j))+g(idx(x,k)),k=1..j)),j=1..n):
r11 := unproduct(t11, x, i=1..n, [], `*`, kb, kb);

t12 := Product(exp(Sum(f(idx(x,j),j,k)+g(idx(x,k),j,k),k=1..j)),j=1..n):
r12 := unproduct(t12, x, i=1..n, [], `*`, kb, kb);

# this is sort-of buggy as well.
t13 := Product(f(idx(x,5))*g(idx(x,7))*h(j),j=1..n):
r13 := unproduct(t13, x, i=1..n, [], `*`, kb, kb);

# Bug!
# t14 := Product(idx(x,2*j),j=1..n/2):
# r14 := unproduct(t14, x, i=1..n, [], `*`, kb, kb);

t15 := Product(f(idx(x,j),j)*g(idx(x,n+1-j),j)*h(j),j=1..n):
r15 := unproduct(t15, x, i=1..n, [], `*`, kb, kb);

t16 := Product(piecewise(a<b,f(idx(x,j),j),g(idx(x,n+1-j),j)),j=1..n):
r16 := unproduct(t16, x, i=1..n, [], `*`, kb, kb);

t17 := Product(5^idx(x,j)*f(idx(x,j))*g(idx(x,n+1-j)),j=1..n):
r17 := unproduct(t17, x, i=1..n, [], `*`, kb, kb);


# File: RigidMotionsParameterSpaceDecompostionRecursive.mpl
#
# Description:
#  This file contains functions used to obtain an arrangement 6 dimensional parameter space of 3D
#  digitized rigid motions.
#  This code has been written for research propose and its aim is to calculate a particular
#  arrangement of quadrics. Therefore, it can or it cannot be useful in study of generic
#  arrangements. The final output are sample points of full dimensional open cells.
#
#  The code was written in relation with the paper: Kacper Pluta, Guillaume Moroz, Yukiko
#  Kenmochi, Pascal Romon, Quadric arrangement in classifying rigid motions of a 3D digital image,
#  2016, https://hal.archives-ouvertes.fr/hal-01334257 referred late on as [Quadrics:2016].
#
# Author:
#  Kacper Pluta - kacper.pluta@esiee.fr
#  Laboratoire d'Informatique Gaspard-Monge - LIGM, A3SI, France
#
# Date:
#  12/24/2016
#
# License:
#  Simplified BSD License
#
# Copyright (c) 2016, Kacper Pluta
# All rights reserved.

# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#   * Redistributions of source code must retain the above copyright
#     notice, this list of conditions and the following disclaimer.
#   * Redistributions in binary form must reproduce the above copyright
#     notice, this list of conditions and the following disclaimer in the
#     documentation and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL Kacper Pluta BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
#
RigidMotionsParameterSpaceDecompostionRecursive := module()
  option package;

  (*Threshold which controls when to synchronize databases.*)
  local RECORDS_TO_SYNCH := 1000;

  uses   RigidMotionsParameterSpaceCommon;
  local  ComputeEventsAType2D, ComputeEventsBType2D, ComputeEventsAType1D,
         ComputeAsymptoticAAEvents2D, ComputeEventsAlgebraicNumbers2D, ComputeSamplePoints2D;

  export IsAsymptotic2D, LaunchComputeSamplePoints2D;

# Procedure: ComputeEventsAType2D
#   Compute events such that a sweep line is tangent to a conic with respect to the first provided
#   variable.
#
# Parameters:
#   Q2D2       - a set of conics
#   grid       - if true then computations are performed in the parallel computation framework
#                called Grid (see Maple documentation)
#   vars2D       - a list of the variables
#
# Output:
#   list of couples: univariate polynomial and index of the generating conic: [poly, [index]]
#
# Comment/Limitations:
#  - Univariate polynomials are expressed in the first variable.
ComputeEventsAType2D := proc(Q2D2, grid::boolean, vars2D::list)
  uses ArrayTools;
  local result := Array([]), s;
  if nops(vars2D) < 2 then
    error "Only systems in at least two variables are supported. The variables are: %1.", vars2D;
  fi:
  s := proc(i::integer, vars2D::list)
    local sys, univ, sol:
    local q := Q2D2[i];
    sys := [q, diff( q, vars2D[2] )];
    univ := UnivariatePolynomial(sys, vars2D):
    return SerializeEvents(GenerateEvents(univ, [i]));
  end proc;
  if grid then
    map[inplace](proc(x) Extend(result, x, inplace=true) end proc, [Grid:-Seq( s( i, vars2D ),
                                                                   i=1..nops( Q2D2 ) )]);
  else
    map[inplace](proc(x) Extend(result, x, inplace=true) end proc, [seq( s( i, vars2D ),
                                                             i=1..nops( Q2D2 ) )]);
  fi;
  return ReconstructEvents(result);
end proc:

# Procedure: ComputeEventsBType2D
#   Compute events such that two conics intersects in a point.
#
# Parameters:
#   Q2D2       - a set of conics
#   grid       - if true then computations are performed in the parallel computation framework
#                called Grid (see Maple documentation)
#   vars2D       - a list of the variables
#
# Output:
#   list of couples: univariate polynomial and indices of the generating conics: [poly,
#   [index1,index2]]
#
# Comment/Limitations:
#  - Univariate polynomials are expressed in the first variable.
ComputeEventsBType2D := proc(Q2D2, grid::boolean, vars2D::list)
  uses ArrayTools;
  local result := Array([]), s:
  s := proc (i, j, vars2D::list)
    local p, sol, univ, sys;
    sys := [Q2D2[i], Q2D2[j]];
    univ := UnivariatePolynomial(sys, vars2D);
    return SerializeEvents(GenerateEvents(univ, [i, j]));
  end proc;
  if grid then
    map[inplace](proc(x) Extend(result, x, inplace=true) end proc, [Grid:-Seq( seq( s(i, j, vars2D),
                                                       j=i+1..nops( Q2D2 ) ), i=1..nops( Q2D2 ) )]);
  else
    map[inplace](proc(x) Extend(result, x, inplace=true) end proc, [seq( seq( s(i, j, vars2D),
                                                 j=i+1..nops( Q2D2 ) ), i=1..nops( Q2D2 ) )]);
  fi;
  return ReconstructEvents(result);
end proc:

# Procedure: IsAsymptotic2D
#   Checks if a curve is an asymptote.
#
# Parameters:
#   p          - a curve given as a polynomial
#   var        - variable to be reduced
#
# Output:
#   Monomial such that there exists a line tangent to the curve at infinity represented in a
#   variable different than 'var' or a constant.
IsAsymptotic2D := proc(p::polynom, var)
  return lcoeff(p, var);
end proc:

# Procedure: ComputeAsymptoticAAEvents2D
#   Compute real algebraic numbers which corresponds to asymptotic cases given by one curve.
#
# Parameters:
#   Q2D2          - a set of conics
#   vars2D       - a list of the variables
#
# Output: Real
#   A list of Events (see EventType).
ComputeAsymptoticAAEvents2D := proc(Q2D2, vars2D::list)
  uses ArrayTools;
  local result := Array([]), s;
  s:=proc(i::integer, vars2D::list)
    local asy;
    asy := RigidMotionsParameterSpaceDecompostionRecursive:-IsAsymptotic2D(Q2D2[i], vars2D[-1]);
    return GenerateEvents(asy, [i]);
  end proc:
  map[inplace](proc(x) Extend(result, x, inplace=true) end proc, [seq(s(i, vars2D),
                                                                i=1..nops(Q2D2))]);
  return result;
end:

# Procedure: ComputeEventsAlgebraicNumbers2D
#   Compute and sort events
#
# Parameters:
#   Q2D2     - set of conics
#   grid       - if true then computations are performed in the parallel computation framework
#                called Grid (see Maple documentation)
#   vars2D       - a list of the variables
#
# Output:
#   Sorted Array of real algebraic numbers
ComputeEventsAlgebraicNumbers2D := proc(Q2D2, grid::boolean, vars2D::list)
  uses ArrayTools;
  local events2D := Array([]);
  Extend(events2D, ComputeEventsAType2D(Q2D2, grid, vars2D), inplace=true);
  Extend(events2D, ComputeEventsBType2D(Q2D2, grid, vars2D), inplace=true);
  Extend(events2D, ComputeAsymptoticAAEvents2D(Q2D2, vars2D), inplace=true);
  return AlgebraicSort(events2D);
end proc:

# Procedure: ComputeEventsAType1D
#   Compute and sort events as algebraic numbers
#
# Parameters:
#   Q2D2     - a list of conics
#
# Output:
#   Sorted list of real algebraic numbers without repetitions.
ComputeEventsAType1D := proc(Q2D2::list)
  local q, factored, sqrFree, rootsF, rf, numbers := Array([]);
  for q in Q2D2 do
    if RootFinding:-HasRealRoots(q) then
      factored := factors( q )[2,..,1];
      for sqrFree in factored do
        rootsF := RootFinding:-Isolate(sqrFree, output='interval');
        for rf in rootsF do
          ArrayTools:-Append(numbers, RealAlgebraicNumber(sqrFree, op(rf)[2][1],
                             op(rf)[2][2]));
        od;
      od;
   fi;
  od;
  numbers := AlgebraicSort(numbers);
  return ListTools:-MakeUnique(convert(numbers, list), 1, proc(a,b) evalb(Compare(a,b) = 0) end proc);
end proc:

# Procedure: ComputeSamplePoints2D
#   Computes sample points for rotational part of rigid motions
#
# Parameters:
#   Q2D         - a list of conics
#   events2D    - each element contains a list of algebraically unique events
#   first       - integer value which indicates the first event to proceed.
#   last        - integer value which indicates the last event to proceed.
#   vars2D      - list of variables in which conics are expressed
#   db          - an instance of the class ComputationRegister
#
# Output:
#   It populates a database, given by databasePath, with sample points.
ComputeSamplePoints2D := proc(Q2D, events2D::Array, first::integer, last::integer,
                              vars2D::list, amid::rational, db::ComputationRegister)
  local i::integer, j::integer, x::list, midpoint::rational, sys::list, records := 0;
  local disjointEvent::list, oneD::list, oneDNeg::list, ranumI, ranumJ;
  if first < 0 or last < 0 or last < first or upperbound(events2D) <= last then
    error "Bounds of the array range are incorrect.":
  end if:
  for i from first to last do
    sys := Q2D[GetQuadrics(events2D[i])];
    ranumI := GetRealAlgebraicNumber(events2D[i]);
    ranumJ := GetRealAlgebraicNumber(events2D[i+1]);
    disjointEvent:=DisjointRanges(ranumI, ranumJ);
    midpoint := (GetInterval(disjointEvent[1])[2] + GetInterval(disjointEvent[2])[1])/2:
    # never call eval with sets!
    sys := eval(sys, vars2D[1] = midpoint);
    oneD := ComputeEventsAType1D(sys);
    oneD := select(proc(x) return evalb(GetInterval(x)[2] >= 0); end proc, oneD);

    if upperbound(oneD) > 0 then
      for j from 1 to upperbound(oneD) - 1 do
        disjointEvent := DisjointRanges(oneD[j],oneD[j+1]);
        InsertSamplePoint(db, [amid, midpoint, (GetInterval(disjointEvent[1])[2] +
                                            GetInterval(disjointEvent[2])[1])/2]);
        records := records + 1;
        if records mod RECORDS_TO_SYNCH = 0 then
          SynchronizeSamplePoints(db);
        fi;
      od:
      InsertSamplePoint(db, [amid, midpoint, GetInterval(oneD[-1])[2] + 1/2]);
      records := records + 1;
      if records mod RECORDS_TO_SYNCH = 0 then
        SynchronizeSamplePoints(db);
      fi;
    fi;
 od:
end proc:

# Procedure: ComputeSamplePoints2D
#   Computes sample points for rotational part of rigid motions using the grid framework
#
#
# Parameters:
#   s         - a list of conics
#   midpoint  - the first dimensional midpoint obtained from the 3D decomposition
#   nodes     - number of nodes used in the parallel computations
#   grid      - a control variable for parallel computations. If true and additional conditions on
#               the size of the problem are fulfilled the problem is solved in the grid framework
#   variables - list of variables in which the problem is expressed
#   db        - an instance of ComputationRegister which provides interface to the database
# Output:
#   It populates a database with sample points.
LaunchComputeSamplePoints2D := proc (s::list, midpoint::rational, nodes::integer,
                                           grid::boolean, variables::list, db::ComputationRegister)
  local events2D, Q2D := ListTools:-MakeUnique([op(variables), op(s)]);
  if grid and nops(s) > 20 then
     events2D := ComputeEventsAlgebraicNumbers2D(Q2D, true, variables);
  else
     events2D := ComputeEventsAlgebraicNumbers2D(Q2D, false, variables);
  fi;
  events2D := remove[flatten](proc(x) evalb(GetInterval(GetRealAlgebraicNumber(x))[2] < 0) end proc,
                            events2D);
  if upperbound(events2D) = 0 then
    return NULL;
  fi;
  events2D := ReduceEvents(events2D);
  AdjustEvents(events2D, upperbound(Q2D), variables);
  ComputeSamplePoints2D(Q2D, events2D, 1, upperbound(events2D) - 1, variables, midpoint, db);
end proc:

end module;


# Use this to define the canonical_labeling function in maple:

canonical_labeling := define_external('canonical_labeling', \
colored_edges::(ARRAY(1..3*m, integer[4])), \ # colored edges as array
\ # Format: ( va_1, vb_1, w_1, va_2, vb_2, w_2, ... )
\ # For a graph with edges (va_k, vb_k) having colors w_k.
ext_vertices::(ARRAY(1..l,integer[4])), \ # external vertices
\ # Array of vertices with another color ( ve_1, ve_2, ... )
lab::(ARRAY(1..n,integer[4])), \ # canonical labeling returned
\ # Canonical labelling is written into this.
n::(integer[4]), \ # number of vtcs
m::(integer[4]), \ # number of edges
w::(integer[4]), \ # number of colors
l::(integer[4]), \ # number of ext vertices
LIB="nauty_wrapper.so" ):

# Example:

lab:=Array([0,1,2,3,4],datatype=integer[4]):
ext:=Array([4],datatype=integer[4]):

G:=Array([0,3,0, 0,2,0, 2,1,0, 3,1,0, 3,4,0], datatype=integer[4]):

canonical_labeling(G, ext, lab, 5, 5, 1, 1);

lab;

G:=Array(subs([1=2,2=3,3=4,4=1],[0,3,0, 0,2,0, 2,1,0, 3,1,0, 3,4,0]), datatype=integer[4]):

canonical_labeling(G, ext, lab, 5, 5, 1, 1);

lab;


$Amt:10.0
$maxAmt:120.0
$multipler:1.002
$additional:0.03
$interval:15.0

main()inline{

	once{
		RemoveImages(Self());
		SetImage(Self(), "main", "CustomEmitter");
		SetTextSize(0.6);
		SetDigitalis(CurrentCoords(), 1);
		SetUnitAttribute(Self(), CONST_NULLIFIERDAMAGES, False);
	}

	if(GetDigitalis(CurrentCoords())!=1) Amt*=0.85;

	SetDigitalisGrowth(CurrentCoords(), True);
	SetDigitalis(CurrentCoords(), 1);

	if(Amt<5)Destroy(Self(), 2);

	SetPopupText("Amt: " $ round(Amt, 2) $ endl() $ "Interval: " $ round(interval/30.0, 2));
	SetImageScale(Self(), "main", dup(Amt/maxAmt*1.7+0.3));
	AddCreeper(CurrentCoords(), Amt);

	if(Amt>=50)once enable = True;

	if(enable){
		[x, y] = RandCoordsInRange(CurrentCoords(), 10);
		noEnemy=True;
		do(GetEnemyUnitsInRange(x, y, 8)){
			pop(); // remove uid from stack
			noEnemy=False;
		}
		if(GetCreeper(x, y)>1 && noEnemy){
			Amt/=1.75;
			uid = CreateUnit("CRPLCORE", x, y);
			AddScriptToUnit(uid, "emitter.crpl");
			if(Amt<maxAmt/2)enable = False;
		}
	}

	if(Amt < maxAmt) Amt = Amt*multipler + additional;
	else once Amt = maxAmt;

	Delay(interval);
}


read("bssn_spher.mpl");

read("built_eqns.mpl");

read("compactify.mpl");

read("perform_fd.mpl");

read("built_direct_ire_test.mpl");


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


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

`is_element/standard_stasheff_star` := (n::posint) -> proc(t)
 local JJ,TT,J,T,u,i,j,k;
 global reason;

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

 JJ := {seq(seq({seq(k,k=i..j)},j=i..n),i=1..n)};

 if map(op,{indices(t)}) <> JJ then
  reason := [convert(procname,string),"t is not indexed by the set of intervals",t,JJ];
  return false;
 fi;

 TT := NULL;

 for J in JJ do
  u := t[J];

  if not (`is_element/RR`(u) and u >= 0 and u <= 1) then
   reason := [convert(procname,string),"t[J] is not in the unit interval",J,u];
   return false;
  fi;

  if (nops(J) = 1 or nops(J) = n) and u <> 1 then
   return false;
  fi;

  if u > 0 then TT := TT,J; fi;
 od;
 TT := {TT};

 if not(`is_element/trees`({seq(i,i=1..n)})(TT)) then
  reason := [convert(procname,string),"TT is not a tree",TT,reason];
  return false;
 fi;

 return true;
end;

`is_equal/standard_stasheff_star` := (n::posint) -> proc(t,u)
 local JJ,J,i,j,k;

 JJ := {seq(seq({seq(k,k=i..j)},j=i..n),i=1..n)};

 for J in JJ do
  if t[J] <> u[J] then
   return false;
  fi;
 od;
 return true;
end:

`is_leq/standard_stasheff_star` := NULL;

`random_element/standard_stasheff_star` := (n::posint) -> proc()
 local JJ,TT,T,t,i,j,k,d;

 d := 12;

 JJ := {seq(seq({seq(k,k=i..j)},j=i..n),i=1..n)};
 TT := `random_element/standard_stasheff_trees`(n)();
 t := table();

 for T in JJ do t[T] := 0; od;
 t[{seq(i,i=1..n)}] := 1;
 for i from 1 to n do t[{i}] := 1; od;

 for T in TT do
  if nops(T) > 1 and nops(T) < n then
   t[T] := rand(0..d)()/d;
  fi;
 od;

 return eval(t);
end;

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