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

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

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

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


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

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

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

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

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

# Ergebnisse der Kinematik für parallelen Roboter laden

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

# Maple-Export

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

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

# PKM-Dynamik-Funktionen mit MPV bereits eingesetzt

# Maple-Export

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 return true;
end:

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

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

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

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

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

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

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

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

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

 return UV;
end:

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 return true;
end:

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

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

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

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


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

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

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

answer1 := total;

# part 2

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

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

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

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

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

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

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

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

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

    return parse(tmp);

end proc:

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


# Domain types which are registered/unregistered with TypeTools.
# Note that the types have to be quoted (additionally to the quote one would
# normally place on certain types) to work properly
local DomainTypes := table(
       # Domain bounds
       [(DomBoundVar = 'DomBoundVar_type')
       ,(DomBoundRange = 'DomBoundRange_type')
       ,(DomBoundBinder = ''DInto(DomBoundVar, DomBoundRange, DomBoundKind)'' )
       ,(DomBoundKind   = 'And(name, satisfies(nm->assigned(Domain:-ExtBound[nm])))' )
       ,(DomBound       = ''And(specfunc(DBound)
                               ,{anyfunc(list(DomBoundBinder))
                                ,anyfunc(list(DomBoundBinder),DomCtx)
                                ,anyfunc(list(DomBoundBinder),DomCtx,table) })'' )
       # Domain shape
       ,(DomConstrain = 'specfunc(relation, `DConstrain`)' )
       ,(DomSum       = 'specfunc(DomShape, `DSum`)' )
       ,(DomSplit     = ''DSplit(Partition(DomShape))'' )
       ,(DomInto      = ''Or(DInto(DomBoundVar, DomBoundRange, DomShape)
                            ,DInto(DomBoundVar, DomShape) )'' )
       ,(DomShape     = 'Or( DomConstrain, DomSum, DomSplit, DomInto )' )
       # Domain
       ,(DomCtx = ''set({relation, `::`})'')
       ,(HDomain = ''DOMAIN(DomBound, DomShape)'' )
       # Maybe domain
       ,(DomNoSol  = ''Not(freeof(`DNoSol`))'' )
       ,(HDomain_mb = ''Or(HDomain, DOMAIN(DomBound, DomNoSol))'' )
       ] );

local DomBoundVar_type := proc(nm)
    local ixs := [indices(Domain:-ExtBound, nolist)], vs;
    ormap(i->type(nm,Domain:-ExtBound[i]:-VarType), ixs);
end proc;

local DomBoundRange_type := proc(nm)
    local ixs := [indices(Domain:-ExtBound, nolist)], vs;
    ormap(i->type(nm,Domain:-ExtBound[i]:-RangeType), ixs);
end proc;

export DOMAIN := proc()
    local errs := indets([args[2..-1]], DomNoSol);
    if errs <> {} then
        'procname'(args[1], 'DNoSol'(map(op,errs)[]));
    end if;
    'procname'(args);
end proc;

export DBound := proc(a)
    local b,cs;
    b := `if`(nargs>=2,[args[2]],[{}])[];
    cs := `if`(nargs>=3,[args[3..-1]],[])[];
    'procname'(a,b,cs) ;
end proc;

export DConstrain := proc()
    local as := {args};
    if false in as then return DSum() end if;
    as := remove(x->x::identical(false),as);
    'procname'(op(as));
end proc;

export DSum := proc()
    local as := [args];
    as := subsindets(as, specfunc(`DSum`), xs->
                     map(x->if op(0,x)=`DSum` then op(x) else x end if,
                         xs));
    if nops(as) = 1 then return op(1,as) end if;
    'procname'(op(as));
end proc;


kernelopts(printbytes=false);
with(RigidMotionsRecoverNMM);
LaunchComputeNMM([a,b,c], "N2", [-2, -1, 0, 1, 2], "__REPLACE__");
done


 func $band32I (
  var %i i32
  ) i32 {
   return (
     band i32(dread i32 %i,
       constval i32 0x12))}
 func $band32I_2 (
  var %i i32
  ) i32 {
   return (
     band i32(dread i32 %i,
       constval i32 0x112))}
 func $band32RR (
  var %i i32, var %j i32
  ) i32 {
   return (
     band i32(dread i32 %i,
       dread i32 %j))}

 func $band64I (
  var %i i64
  ) i64 {
   return (
     band i64(dread i64 %i,
       constval i64 0x1200000015))}
 func $band64I_2 (
  var %i i64
  ) i64 {
   return (
     band i64(dread i64 %i,
       constval i64 0x1200000115))}
 func $band64RR (
  var %i i64,
  var %j i64
  ) i64 {
   return (
     band i64(dread i64 %i,
              dread i64 %j))}
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


# Roboter aus Viergelenkkette und Schubgelenk
# Voraussetzung:
# Viergelenkkette muss mit der Toolbox berechnet worden sein (Arbeitsblatt "vgk_kinematic_constraints.mw")
# Autor
# Abderahman Bejaoui
# Studienarbeit bei: Moritz Schappler, moritz.schappler@imes.uni-hannover.de, 2018-08
# (C) Institut fuer mechatronische Systeme, Leibniz Universitaet Hannover
# Quellen
# TODO
#
# Initialisierung
# Import
restart:
kin_constraints_exist := true: # Für Speicherung
;
with(StringTools): # Für Zeitausgabe
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
#with(ListTools):
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 "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../robot_codegen_constraints/proc_subs_kintmp_exp":
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 Nebenstruktur nur in Abhängigkeit der verallgemeinerten Koordinaten
kintmp_qs := Matrix(RowDimension(kintmp_s),1):
qJ_t:= <qJ1(t),qJ2(t),qJ3(t),qJ4(t),qJ5(t)>:
qJ_s:= <qJ1s,qJ2s,qJ3s,qJ4s,qJ5s>:
# Konstante Winkel bereits hineinschreiben
for i from 1 to RowDimension(kintmp_s) do
  if diff(kintmp_s(i,1), t) = 0 then
    kintmp_qs(i,1) := kintmp_s(i,1):
  end if:
end do:
# Variablen definieren für die Hilfswinkel
#
# Ersetzungsausdrücke definieren.
# Speichere Sinus und Cosinus der Winkel direkt ab, da diese in den Rotationsmatrizen direkt auftreten.
# Spalte 1: Zu suchender Ausdruck (sin oder cos eines Winkels)
# Spalte 2: Einzusetzender Ausdruck.
# Dadurch werden arctan-Ausdrücke in der direkten Kinematik reduziert.
# Ähnliches Vorgehen wie in [1].
kintmp_subsexp := Matrix(2*RowDimension(kintmp_s),2):
for i from 1 to RowDimension(kintmp_s) do
  kintmp_subsexp(2*i-1, 1) := sin(kintmp_s(i,1)):
  kintmp_subsexp(2*i,   1) := cos(kintmp_s(i,1)):
  # Initialisierung der rechten Spalte mit gleichen Werten. Später nur Ersetzung, wenn Vorteilhaft.
  kintmp_subsexp(2*i-1, 2) := kintmp_subsexp(2*i-1, 1):
  kintmp_subsexp(2*i,   2) := kintmp_subsexp(2*i,   1):
end do:
read sprintf("../codeexport/fourbar1TE/tmp/fourbar1TE_angles_trig_elim"):
# winkel
;
winkel(..,2):
# Viergelenkkette : A-B-C-D
winkel1:=<<sin_rho34|sin(rho34_s)>;<cos_rho34|cos(rho34_s)>;<sin_rho12|sin(rho12_s)>;<cos_rho12|cos(rho12_s)>;<sin_rho1|sin(rho1_s)>;<cos_rho1|cos(rho1_s)>>:
for i from 1 to 6 do
	winkel1(i,2):=winkel(i,2):
     cos_phi_s :=sin(qJ5s):
	sin_phi_s := cos(qJ5s):
	winkel1(i,2):=subs({sin(qJ1s)=sin_phi_s},winkel1(i,2)):
	winkel1(i,2):=subs({cos(qJ1s)=cos_phi_s},winkel1(i,2)):
     winkel1(i,2) := subs({l1=AB}, winkel1(i,2)):
     winkel1(i,2) := subs({l2=AD}, winkel1(i,2)):
     winkel1(i,2) := subs({l3=qJ2s+HC}, winkel1(i,2)):
     winkel1(i,2) := subs({l4=CB}, winkel1(i,2)):
end do:
sin_rho34_s:=winkel1(1,2):
cos_rho34_s:=winkel1(2,2):
sin_rho12_s:=winkel1(3,2):
cos_rho12_s:=winkel1(4,2):
sin_rho1_s:=winkel1(5,2):
cos_rho1_s:=winkel1(6,2):
sin_rho48_s:=sin_rho1_s:
cos_rho48_s:=cos_rho1_s:
# Kitmp_subsexp
kintmp_subsexp(1,2):=sin_rho12_s:
kintmp_subsexp(2,2):=cos_rho12_s:
kintmp_subsexp(3,2):=sin_rho34_s:
kintmp_subsexp(4,2):=cos_rho34_s:
kintmp_subsexp(5,2):=sin_rho48_s:
kintmp_subsexp(6,2):=cos_rho48_s:
# Kintmp_qs
kintmp_qs(1,1):=arctan(sin_rho12_s,cos_rho12_s):
kintmp_qs(2,1):=arctan(sin_rho34_s,cos_rho34_s):
kintmp_qs(3,1):=arctan(sin_rho48_s,cos_rho48_s):
# Export
kintmp_qt := convert_s_t(kintmp_qs):
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). %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
for i from 1 to RowDimension(kintmp_s) do
  tmp := kintmp_qs(i):
  save tmp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert_kintmpq_%d",robot_name, i):
  save tmp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert_kintmpq_%d.m", robot_name, i):
end do:
save kin_constraints_exist, kintmp_qs, kintmp_qt,kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert" ,robot_name):
save kin_constraints_exist, kintmp_qs, kintmp_qt, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
save kintmp_qs, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_qs_maple_inert", robot_name):
#printf("Ausdrücke mit Inert-Arctan exportiert (Matlab). %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Liste mit abhängigen konstanten Kinematikparametern erstellen (wichtig für Matlab-Funktionsgenerierung)
read "../helper/proc_list_constant_expressions";
kc_symbols := Matrix(list_constant_expressions( kintmp_subsexp ));
#kc_symbols :=Transpose(kc_symbols);
save kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_maple", robot_name):
MatlabExport(kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_matlab.m",robot_name),2);


 func $foo (
  var %i i32
#var %i1 i32, var %j1 i32, var %k1 i32
  ) i32 {
   return (
     add i32(dread i32 %i,
       constval i32 -998))}

 func $foo1 (
  var %i i32, var %j i32, var %k i32,
  var %i1 i32, var %j1 i32, var %k1 i32
  ) i32 {
   return (
     add i32(dread i32 %i, dread i32 %j))}

 func $foo2 (
  var %i i32, var %j i32, var %k i32
  ) i32 {
   return (
     add i32(dread i32 %i, constval i32 0x111111111))}

 func $foo3 (
  var %i i64, var %j i64, var %k i32
  ) i64 {
   return (
     add i64(dread i64 %i, constval i64 0x111111111))}

 func $foo4 (
  var %i i64, var %j i64, var %k i32
  )i64  {
   return (
     add i64(dread i64 %i, dread i64 %j))}

 func $foo5 (
  var %i i64, var %j i64, var %k i32
  ) i64 {
   return (
     add i64(dread i64 %i, constval i64 0x11111))}

 func $foo6 (
  var %i f64, var %j f64, var %k i32
  ) f64 {
   return (
     add f64(dread f64 %i, constval f64 2.237))}

 func $foo66 (
  var %i f64, var %j f64, var %k i32
  ) f64 {
   return (
     add f64(dread f64 %i, constval f64 2.237))}
 func $foo7 (
  var %i f32, var %j f32, var %k i32
  ) f32 {
   return (
     add f32(dread f32 %i, dread f32 %j))}

 func $foo8 (
  var %i f32, var %j f32, var %k i32
  ) f32 {
   return (
     add f32(dread f32 %i, constval f32 2.237f))}

 func $foo9 (
  var %i i8, var %j u8, var %k i32) i32 {
   return (
     add i32(dread i32 %i, dread u32 %j))}
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


 func $foo (
  var %i i32
#var %i1 i32, var %j1 i32, var %k1 i32
  ) i32 {
   return (
     extractbits i32 1 23(
       constval i32 -998))}

 func $foo1 (
  var %i i32, var %j i32, var %k i32,
  var %i1 i32, var %j1 i32, var %k1 i32
  ) i32 {
   return (
     extractbits i32 0 5(dread i32 %i))}

 func $foo11 (
  var %i i32, var %j i32, var %k i32,
  var %i1 i32, var %j1 i32, var %k1 i32
  ) u32 {
   return (
     extractbits u32 0 5(dread i32 %i))}

 func $foo2 (
  var %i i32, var %j i32, var %k i32
  ) i32 {
   return (
     extractbits i32 3 20(constval i32 0x111111111))}

 func $foo3 (
  var %i i64, var %j i64, var %k i32
  ) i64 {
   return (
     extractbits i64 5 50(dread i64 %i))}

 func $foo4 (
  var %i i64, var %j i64, var %k i32
  )i64  {
   return (
     extractbits i64 6 44 (dread i64 %i))}

 func $foo5 (
  var %i i64, var %j i64, var %k i32
  ) i64 {
   return (
     extractbits i64 1 10(constval i64 0x11111))}

 func $foo9 (
  var %i i8, var %j u8, var %k i32) u32 {
   return (
     extractbits u32 1 31(dread u32 %j))}

 func $foo51 (
  var %i u64
  ) u64 {
   return (
     extractbits u64 0 15(constval i64 0x11111))}

 func $foo52 (
  var %i i32
  ) i32 {
   return (
     sext i32 11 (constval i32 99))}

 func $foo53 (
  var %i u64
  ) u64 {
   return (
     zext u64 11 (constval u64 88))}

 func $foo54 (
  var %i i32
  ) i32 {
   return (
     sext i32 11 (dread i32 %i))}

 func $foo55 (
  var %i u64
  ) u64 {
   return (
     zext u64 41 (dread u64 %i))}
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


`is_element/BW1` := eval(`is_element/prime_simplex`);
`is_equal/BW1` := eval(`is_equal/prime_simplex`);
`is_leq/BW1` := NULL;
`random_element/BW1` := eval(`random_element/prime_simplex`);
`list_elements/BW1` := NULL;
`count_elements/BW1` := NULL;
`phi/nonempty_subsets/BW1` := eval(`phi/nonempty_subsets/prime_simplex`);


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

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

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

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

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

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

 return true;
end;

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

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

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

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

 return true;
end;

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

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

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

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

 return true;

end;

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

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

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

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

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

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

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

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

 return eval(Q);
end:

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

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

 return Q[A];
end;

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

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

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

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

 return true;
end;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 return eval(R);
end;

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

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

 Q := table();

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

 return eval(Q);
end;

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

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

 x := table();

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

 return eval(x);
end;

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

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


#indirect access are associated with pionters and arrays
#iassign<type><field-id>(<addr-expr>, <rhs-expr>)
# type gives the high level type of <addr-expr> and must be a pointer type
# <addr-expr> is computed to return an address
# <rhs-expr> return a value
#iread<prim-type><type><field-id>(<addr-expr>)
#
func $new ( var %n i32 )a64
func $strcpy ( var %dest <*i8>, var %src <*i8> )<*i8>
type $People < struct{
     @name < * i8 >,
     @number < * i32> }>
func $addition ( var %a i32, var %b i32 )i32 {
     return(
	add i32( dread i32 %a, dread i32 %b ))}
var $name < [6] i8 > = [ 104, 117, 97, 119, 101, 105 ]
var $temp<[6] i8> = [ 104, 117, 97, 119, 101, 105 ]
func $main ( ) i32 {
     var %cPtr < * i8 >
     dassign %cPtr ( constval a64 0)
     dassign %cPtr (
     	     addrof a64 $name)

     var %cPtr1 i8
     dassign %cPtr1(
     	     iread i32 < * i8 >( dread a64 %cPtr ))
     dassign %cPtr(
     	     add u64(
	     	 dread a64 %cPtr,
		 constval u64 1))
     var %cPtr2 <* i8>
     dassign %cPtr2(
     	     add u64(
	     	 dread a64 %cPtr,
		 constval u64 1))

     var %iPtr1 <* i32 >
     var %iPtr2 <* i32 >
     iassign < * i32>(
     	     dread a64 %iPtr1,
	     constval i32 10)
     iassign < * i32 >(
     	     dread a64 %iPtr2,
	     iread i32 < * i32> (
	     	   dread a64 %iPtr1))
     var %sPtr <* $People >
     #call $new(
     #	  iread <>)
     call &new (constval u32 128)
     dassign %sPtr(
         regread a64 %%retval
         )
     call &strcpy ( dread a64 %sPtr 0 ,
     	  	    addrof a64 $temp)
     iassign <* i32>(
     	     dread a64 %sPtr 0,
	     constval i32 5)
     var %i i32
     var %j i32
     var %aPtr1 < [10] i32 >
     var %aPtr2	< [10][10] i32 >
     var %aPtr3	< [10][10][10] i32 >
     dassign %i ( constval i32 0 )
     while(
       lt i32 i32 ( dread i32 %i, constval i32 10 )){
       	  iassign < *[10] i32 >(
	  	  array 1 a64 < *[10] i32 >(addrof a64 %aPtr1,
		  		          dread i32 %i),
                  constval i32 10 )

          iassign< *[10] i32 >(
	  	  array 1 a64 < *[10] i32 >( addrof a64 %aPtr1,
		  	                   dread i32 %i),
		  dread i32 %i )

          iassign < *[10] i32 >(
	  	  array 1 a64 < *[10] i32 >( addrof a64 %aPtr1,
		  	      	    	   dread i32 %i ),
		  iread i32 <* i32 >(
		  	addrof a64 %aPtr1 ))
       dassign %i(
	 	 add i32(
		     dread i32 %i,
		     constval i32 1))}

     dassign %i ( constval i32 0 )
     while(
	lt i32 i32 ( dread i32 %i, constval i32 10 )){
	dassign %j ( constval i32 0 )
	while(
		lt i32 i32 ( dread i32 %j, constval i32 10 )){
		   iassign < *[10][10] i32 >(
		   	   array 1 a64 < *[10][10] i32 >( addrof a64 %aPtr2,
		   	   	 dread i32 %i,
				 dread i32 %j),
		           constval i32 10 )
	           iassign < *[10][10] i32 >(
		   	  array 1 a64< *[10][10] i32 >( addrof a64 %aPtr2,
			  	dread i32 %i,
				dread i32 %j),
			  mul i32 (
			      dread i32 %i,
			      dread i32 %j))
		   iassign< *[10][10] i32 >(
		   	    array 1 a64 < *[10][10] i32 >( addrof a64 %aPtr2,
			    	  dread i32 %i,
				  dread i32 %j),
		            iread i32 < *[10] i32 >(
			    	  array 1 a64< *[10] i32 >(
				  	addrof a64 %aPtr1,
				  	rem i32 (
					    mul i32 (
					    	dread i32 %i,
						dread i32 %j),
					    constval i32 10))))
                dassign %j(
			add i32(
		 	    dread i32 %j,
		     	    constval i32 1))}
     dassign %i(
     	     add i32(
	     	 dread i32 %i,
		 constval i32 1))}
     var %k i32
     dassign %i ( constval i32 0)
     while(
	lt i32 i32 ( dread i32 %i, constval i32 10 )){
	dassign %j ( constval i32 0)
	while(
	lt i32 i32 ( dread i32 %j, constval i32 10)){
	   while(
		lt i32 i32 ( dread i32 %k, constval i32 10)){
		iassign < *[10][10][10] i32 >(
			array 1 a64 < *[10][10][10] i32 >(addrof a64 %aPtr3,
			dread i32 %i,
			dread i32 %j,
			dread i32 %k),
			constval i32 10)
		iassign < *[10][10][10] i32 >(
			array 1 a64 < *[10][10][10] i32>(addrof a64 %aPtr3,
			dread i32 %i,
			dread i32 %j,
			dread i32 %k),
			mul i32 (
			    mul i32 (
			    	dread i32 %i,
				dread i32 %j),
			    dread i32 %k))
	        iassign < *[10][10][10] i32 >(
			array 1 a64 < *[10][10][10] i32 >(addrof a64 %aPtr3,
			dread i32 %i,
			dread i32 %j,
			dread i32 %k),
			mul i32 (
			    iread i32 < *[10][10] i32>(
			    	  array 1 a64 <*[10][10] i32>( addrof a64 %aPtr1,
				  dread i32 %i,
				  dread i32 %j)),
				  constval i32 10))
 		dassign %k(
			add i32(
			    dread i32 %k,
			    constval i32 1))}
         dassign %j(
	 	 add i32(
		     dread i32 %j,
		     constval i32 1))}
    dassign %i(
    	    add i32(
	    	dread i32 %i,
		constval i32 1))}

    var %fPtr1 <func( i32, i32) i32 >
    var %fPtr2 <func( i32, i32) i32 >

    dassign %fPtr1(
          addrof a64 $addition)
    dassign %fPtr2(
          addrof a64 $addition)
    var %result i32
    icall ( dread a64 %fPtr1, constval i32 2, constval i32 3)
    dassign %result(
    	    regread i32 %%retval)
    return ( dread i32 %result)}

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


$ifndef _CLASSIFYER_
$define _CLASSIFYER_

$include "Basic.mpl"
$include "Condition.mpl"
$include "Fetch.mpl"
$include "InvOrder.mpl"
$include "InvSimplify.mpl"
$include "Logout.mpl"
$include "Utils.mpl"
$include "InvSol.mpl"
$include "Closure.mpl"
$include "ClassifyHolder.mpl"
$include "GenSol.mpl"
$include "TeqSol.mpl"
$include "TsolsList.mpl"

# 绑定函数
reset:=ClassifyHolder:-reset;
addSol:=ClassifyHolder:-addSol;
getSols:=ClassifyHolder:-getSols;
getIeqCode:=ClassifyHolder:-getIeqCode;
addUnsolvedSol:=ClassifyHolder:-addUnsolvedSol;
getUnsolvedSols:=ClassifyHolder:-getUnsolvedSols;

# 重新求解的入口
classify:=proc(As,A,eqs)
    local sol,n;
    sol:=Object(InvSol);
    sol:-As:=As;
    sol:-A:=A;
    n:=LinearAlgebra:-RowDimension(A);
    sol:-nvars:=n;
    sol:-vars:={seq(a[i],i=1..n)};
    sol:-oeq:=eqs;
    sol:-state:=0;
    reset();
    resolve(sol);
end proc:

# 可重用的求解入口
resolve:=proc(s::InvSol)
    flogf[0](convert(procname,string));
    if      (s:-state=0) then
        return solveOeq(s);     # 针对偏微分方程组进行求解
    elif    (s:-state=1) then
        return solveIeq(s);     # 针对不变量方程进行求解
    elif    (s:-state=2) then
        return checkNewInvariants(s); # 检查是否产生了新的不变量
    elif    (s:-state=3) then
        return solveTeq(s);     # 求解变换方程
    elif    (s:-state=4) then
        return selectRep(s);    # 选择最优代表元
    elif    (s:-state=5) then
        addSol(s);              # 求解成功
        return;
    else
        error "unkown state";
    end if;
end proc:

# 针对偏微分方程组进行求解
solveOeq:=proc(s::InvSol)
    local deltas;
    flogf[0](convert(procname,string));
    deltas:=getNewInvariants(s);
    if deltas=[] or convert(deltas,set) subset convert(s:-Deltas,set) then
        flogf[1]("没有新的不变量");
        solveByClosure(s);
    else
        flogf[1]("解得新的不变量");
        flog[1]~(deltas);
        genIeq(s,deltas);
    end if;
end proc:

# 求解新的不变量
# 两处判断用的都是这个函数，因此要考虑通用性，代入的条件到底是什么，
# 该如何定义，该如何和别的地方进行协调？
getNewInvariants:=proc(s::InvSol)
    local oeq,deltas;
    flogf[0](convert(procname,string));
    flogf[0]("-------------------------------------------------");
    flogf[0]("求解新的不变量");
    flogf[0]("附加约束");
    flog[0](s:-addcons);

    oeq:=getRealOeq(s);
    flogf[0]("偏微分方程组");
    flog[0](oeq);

    # 求解新不变量（已化简）
    deltas:=getInvariants(oeq);

    # 检查是否有解
    if not type(indets(deltas,name),set(specindex(a))) then
        deltas:=[];
    end if;

    return deltas;
end proc:

# 按封闭进行求解
solveByClosure:=proc(s::InvSol)
    local c,s1,s2;
    flogf[0](convert(procname,string));
    c:=getMinClosure(getClosure(s:-A,getZeroCons(s)));
    c:={seq(a[x],x in c)};
    # 封闭不全为零
    flogf[1]("封闭不全为零");
    flog[1](c);
    s1:=Object(s);
    # 添加展示性约束
    s1:-discons:=s1:-discons union {add(x^2,x in c)<>0};
    s1:-rep:=v[op([1,1],c)];
    s1:-state:=5;
    resolve(s1);
    # 封闭全为零
    s2:=Object(s);
    # 这里选择了在addcons中加入信息，而不是加入到解中
    solveClosureAllZero(s2,c);
end proc:

# 处理封闭全为零的情况
solveClosureAllZero:=proc(s::InvSol,c::set(specindex(a)))
    local s1;
    flogf[0](convert(procname,string));
    flogf[1]("-------------------------------------------------");
    flogf[1]("封闭全为零");
    flog[1](c);
    addZeroCons(s,{seq(x=0,x in c)});
    updateVars(s);
    if numelems(s:-vars)>1 then
        solveOeq(s);
    elif numelems(s:-vars)=1 then
        s1:=Object(s);
        s1:-rep:=v[op([1,1],s:-vars)];
        s1:-state:=5;
        resolve(s1);
    else
        return;
    end if;
end proc:

# 生成不变量方程组
genIeq:=proc(s::InvSol,deltas)
    local spos,pos,n;
    flogf[0](convert(procname,string));
    if andmap(type,rhs~(s:-ieq),0) then
        spos:=numelems(s:-Deltas)+1;
        s:-Deltas:=[s:-Deltas[],deltas[]];
        s:-orders:=findInvariantsOrder~(s:-Deltas);
        n:=numelems(s:-Deltas);
        for pos from spos to n do
            buildIeq(s,pos);
        end do;
    else
        appendIeq(s,deltas);
    end if;
end proc:

# 按照拓展的方式生成不变量方程
# 对于 Delta[1]=c[1],...,Delta[n]=c[n]的方程，若生成了新的不变量Delta[n+1]
# 新的方程为Delta[1]=c[1],...,Delta[n]=c[n],Delta[n+1]=c[n+1]
appendIeq:=proc(_s::InvSol,deltas)
    local s,cid,getCname;
    flogf[0](convert(procname,string));
    cid:=findCid(_s);
    getCname:=proc()
        cid:=cid+1;
        return c[cid];
    end proc:
    s:=Object(_s);
    s:-Deltas:=[s:-Deltas[],deltas[]];
    s:-orders:=findInvariantsOrder~(s:-Deltas);
    s:-ieq:=[s:-ieq[],seq(deltas[i]=getCname(),i=1..numelems(deltas))];
    s:-state:=1;
    resolve(s);
end proc:

# 按位置进行讨论，建立不变量方程
# 讨论 Delta[k] 的取值
# 此时 Delta[1..(k-1)]=0，Delta[(k+1)..n]=c[j]
buildIeq:=proc(_s::InvSol,pos::posint)
    local s,n,xpos,cid,getCname,x,rs;
    flogf[0](convert(procname,string));
    n:=numelems(_s:-Deltas);
    if type(_s:-orders[pos],even) then
        xpos:=[1,-1,0];
    else
        xpos:=[1,0];
    end if;
    cid:=0;# 因为前面全是0
    getCname:=proc()
        cid:=cid+1;
        return c[cid];
    end proc:
    rs:=Array(1..n,x->`if`(x>pos,getCname(),0));
    for x in xpos do
        # 只求解全零方程，其它情况将包含在下一个pos的情况中
        if x=0 and pos<> n then
            next;
        end if;
        rs[pos]:=x;
        s:=Object(_s);
        s:-ieqCode:=getIeqCode();
        s:-ieq:=[seq(s:-Deltas[i]=rs[i],i=1..n)];
        s:-state:=1;
        resolve(s);
    end do;
end proc:

# 获取当前c的最后一个下标
# 本做法保证一个不变量方程中的任意常数c，从c[1]开始依次编号
# 而不与其它不变量方程中的任意常数c进行比较
findCid:=proc(s::InvSol)
    local rs:=select(type,rhs~(s:-ieq),specindex(c));
    flogf[0](convert(procname,string));
    if rs=[] then
        return 0;
    else
        return op(1,rs[-1]);
    end if;
end proc:

# 是否等价于封闭全为零的解
isClosureSol:=proc(s::list)
    return andmap(x->evalb(rhs(x)=0 or lhs(x)=rhs(x)),s);
end proc:

# 求解不变量方程
solveIeq:=proc(s::InvSol)
    local isols,icons,acons,tc,cc,discons,subcons,ind,i,isol;
    flogf[0](convert(procname,string));
    flogf[1]("-------------------------------------------------");
    flogf[1]("求解不变量方程");
    displayIeq(s);

    # 针对全部变量求解
    isols,icons:=ieqsolve(s:-ieq,{seq(a[i],i=1..s:-nvars)});

    flog[1](isols);
    flogf[1]("约束条件");
    flog[1](icons);

    # 等价于封闭全为零进行求解
    if andmap(isClosureSol,isols) then
        flogf[1]("等价于封闭为零");
        for isol in isols do
            solveClosureAllZero(Object(s),
            indets(select(x->rhs(x)=0,isol),name));
        end do;
        return;
    end if;

    # 进入封闭的求解条件
    # 若只存在单变量大于零或小于零的的约束，则等价成非零约束进行求解。
    # 只存在常数约束和非零约束
    # 只存在一个具有单个单变量非零约束的一般解
    # 若存在多个具有单个单变量非零约束的一般解，
    #     若两个解不能比较，则选择其中一个进行求解。
    #     若两个解等价，则分别进行封闭求解。
    # 若不能进入封闭进行求解，则一个解对应一个特解分别进行求解

    # 首先满足只存在单变量约束
    acons:=`union`(icons[]);
    if not andmap(x->evalb(numelems(indets(x,name))=1),acons) then
        return branchSolve(s,isols,icons);
    end if;

    # 尝试将大于零或小于零的约束等价为非零约束
    # 单变量约束只有3种可能：= <> <
    acons:=select(type,acons,`<`);
    if acons<>{} then
        tc:=table();
        for cc in acons do
            if lhs(cc)=0 or rhs(cc)=0 then
                tappend(tc,indets(cc,name)[],cc);
            else
                # 存在不是大于零或者小于零的约束，则不进入封闭进行计算
                return branchSolve(s,isols,icons);
            end if;
        end do;
        # 对于同一个变量存在大于零和小于零的约束
        # 一般这个约束会体现在值域的约束中，但是现在还没有计算值域
        # 而且从其次性进行考虑，出现这种值域约束的同时，肯定会出现根号约束，
        # 大部分情况下根号约束是不能处理的，不进行封闭计算。
        # 可以给出一个例子，a[1]^2-a[2]^2=0，可以解出 a[1]=±a[2]
        # 反正太复杂了我不走封闭了
        if ormap(x->evalb(numelems(x)>1),[entries(tc,nolist)]) then
            return branchSolve(s,isols,icons);
        end if;
        discons:={entries(tc,nolist)};
        discons:=map(x->x[],discons);
        subcons:=seq((x)=(indets(x)[]<>0),x in discons);
        # 处理掉<约束之后，只剩下单变量等式约束和非零约束，必然能够进行封闭运算
        s:-discons:=s:-discons union discons;
        icons:=subs(subcons,icons);
    end if;

    # 然后至少要有一个非零约束
    if not ormap(isNonZeroCon,`union`(icons[])) then
        return branchSolve(s,isols,icons);
    end if;

    # 尝试进入封闭
    # TODO 还没加入一般解只有一个非零约束的判定
    ind:=findGenSolInd(icons,s:-nvars);
    if type(ind,posint) then
        # 只有一个极大元
        closureRefine(s,isols,icons,ind);
    else
        # 有多个极大元，在不能比较的部分中选择包含元素个数最少的那组
        ind:=MinSelect(ind,numelems)[];
        for i in ind do
            closureRefine(s,isols,icons,i);
        end do;
    end if;

end proc:

# 按照分支方法进行求解
branchSolve:=proc(s::InvSol,isols,icons)
    local i,n,_s;
    flogf[0](convert(procname,string));
    flogf[1]("分支求解");
    n:=numelems(isols);
    for i from 1 to n do
        _s:=Object(s);
        _s:-isols:=[isols[i]];
        _s:-icons:=[icons[i]];
        _s:-state:=2;
        _s:-isolInd:=1;
        _s:-useBranch:=true;
        resolve(_s);
    end do;
end proc:

# 解的精简，按照封闭求解
# 方程的解至多只含等式约束和非零的约束
closureRefine:=proc(_s::InvSol,isols,icons,ind::posint)
    local CL,zcons,s,s0,s1;
    flogf[0](convert(procname,string));
    flogf[1]("一般解为");
    flog[1](isols[ind]);
    s:=Object(_s);
    # 取封闭
    CL:=getClosure(s:-A,getZeroCons(s));
    zcons:=select(isNonZeroCon,icons[ind]);# 提取约束
    zcons:=indets(zcons,name);# 提取变量
    zcons:=map(x->op(1,x),zcons);# 提取下标
    zcons:=[zcons[]];
    CL:=`union`(CL[zcons][]);
    CL:={seq(a[x],x in CL)};# 系数表示
    flogf[1]("封闭为");
    flog[1](CL);
    # 封闭不全为零
    s1:=Object(s);
    s1:-state:=2;
    s1:-isols:=isols;
    s1:-icons:=icons;
    s1:-isolInd:=ind;
    s1:-discons:=s1:-discons union {add(x^2,x in CL)<>0};
    resolve(s1);
    # 封闭全为零
    if checkIeq(s,CL) then
        flogf[1]("封闭全为零有解");
        s0:=Object(s);
        solveClosureAllZero(s0,CL);
    else
        flogf[1]("封闭全为零无解");
    end if;
end proc:

# 检查封闭全为零是否满足方程
checkIeq:=proc(s::InvSol,c::set(specindex(a)))
    local eq,e;
    try
        eq:=subs(seq(x=0,x in c),s:-ieq);
        eq:=remove(x->lhs(x)=rhs(x),eq);
        for e in eq do
            if not isVar(lhs(e)) then
                if isVar(rhs(e)) then
                    return false;
                else
                    if lhs(x)<>rhs(x) then
                        return false;
                    end if;
                end if;
            end if;
        end do;
        return true;
    catch :
        return false;
    end try;
end proc:

isVar:=proc(x)
    return indets(x,name)<>{};
end proc:

# 取特解
checkNewInvariants:=proc(s::InvSol)
    local deltas;
    flogf[0](convert(procname,string));
    deltas:=getNewInvariants(s);
    if deltas=[] or convert(deltas,set) subset convert(s:-Deltas,set) then
        flogf[1]("没有新的不变量");
        solveRep(s);
    else
        flogf[1]("解得新的不变量");
        flog[1]~(deltas);
        genIeq(s,deltas);
    end if;
end proc:

# 取特解
solveRep:=proc(s::InvSol)
    local rsols,_s,rsol;
    flogf[0](convert(procname,string));
    rsols:=fetchSpecSol~(s:-isols,s:-icons,nonzero);# 针对所有解取特解
    flogf[1]("一般解为");
    flog[1](s:-isols[s:-isolInd]);
    flogf[1]("取特解");
    rsols:=`union`(rsols[]);
    rsols:=convert(rsols,list);
    flog[1](rsols);
    if not s:-useBranch then
        s:-rsols:=rsols;
        s:-state:=3;
        resolve(s);
    else
        for rsol in rsols do
            _s:=Object(s);
            _s:-rsols:=[rsol];
            _s:-state:=3;
            resolve(_s);
        end do;
    end if;
    return;
end proc:

# 建立不变量方程并求解
solveTeq:=proc(s::InvSol)
    local ESC;
    ESC:=map[2](specTeqSolve,s,s:-rsols);
    if not ormap(TeqSol:-hasSol,ESC) then
        WARNING("-----------------------------变换方程均无解\n");
        addUnsolvedSol(s);
        return;
    end if;
    s:-tsols:=ESC;
    s:-state:=4;
    return resolve(s);
end proc:

specTeqSolve:=proc(sol::InvSol,spec::list)
    local ax,_ax,res;
    ax:=Matrix([seq(a[i],i=1..sol:-nvars)]);
    _ax:=Matrix(spec);
    res:=TeqSol(spec,[solveSpecTeq(ax,_ax,sol),
                      solveSpecTeq(_ax,ax,sol)]);
    return res;
end proc:

solveSpecTeq:=proc(va,vb,s::InvSol)
    local teq,tsol,tcon,var,n,eqs,eq,_eq,_con,_sol,needResolve;
    n:=numelems(va);
    teq:=subs(s:-isols[s:-isolInd][],convert(va-vb.s:-A,list));
    var:={seq(epsilon[i],i=1..n)};
    flogf[0]("求解变换方程");
    flog[0](teq);
    needResolve:=false;
    try # 设置最长求解时间
        tsol:=timelimit(3,teqsolve(teq,var));
    catch:
        flogf[1]("直接求解超时");
        flog[1](teq);
        needResolve:=true;
    end try;
    if needResolve or tsol=[] then
        # 求解失败，尝试二次求解法方法
        # 首次求解
        eqs:=teqsolve(teq);
        # 二次求解
        tsol:=[];
        tcon:=[];
        for eq in eqs do
            if not checkTeqSol(teq,eq) then
                _sol:=[[]];
                _con:=[{}];
            else
                _eq,_con:=selectremove(has,eq,epsilon);
                _con:=remove(x->type(x,`=`) and (lhs(x)=rhs(x)),_con);
                _con:=convert(_con,set);
                _sol:=teqsolve(_eq,var,_explicit);
                _con:=map(x->getTsolCons(x,s) union _con,_sol);
            end if;
            tsol:=[tsol[],_sol[]];
            tcon:=[tcon[],_con[]];
        end do;
    else
        # 求解成功，直接计算约束
        tcon:=map(x->getTsolCons(x,s),tsol);
    end if;
    # 清理矛盾解
    tsol:=zip((s,c)->if (undefined in rhs~(c)) then NULL else s end if,
             tsol,tcon);
    tcon:=remove(c->(undefined in rhs~(c)),tcon);
    return [teq,tsol,tcon];
end proc:

# 检查解是否满足条件
checkTeqSol:=proc(teq,sol)
    return andmap(type,RealDomain:-simplify(subs(sol[],teq)),0);
end proc:

# 自定义变换方程求解函数
teqsolve:=proc({_explicit::boolean:=false})
    local res;
    if _explicit then
        res:=[RealDomain:-solve(_rest,explicit)];
    else
        res:=convert~([RealDomain:-solve(_rest)],radical);
    end if;
    return convert~(res,list);
end proc:

# 获取变换方程的解的约束条件
# 删除只含 epsilon 的约束
# 删除 s:-discons 主要是为了删除只能 a[k]>0 的约束，这依赖于 solveIeq 的相关操作。
# 删除通解中存在的约束
getTsolCons:=proc(tsol,s::InvSol)
    return (select(has,findSolutionDomain(tsol),{a,c}) minus getIsolCons(s));
end proc:

# 检查是否是非零约束
# 只考虑单变量非零约束
# 这里只考虑 x<>0 的形式，不考虑 0<>x 的情况
# 因为在求解约束条件时，能够保证把 0<>x 变成 x<>0
isNonZeroCon:=proc(x)
    return op(0,x)=`<>` and rhs(x)=0 and numelems(indets(x,name))=1;
end proc:

# 不变量方程的求解函数
ieqsolve:=proc(eq::list,vars::set)
    local isols,icons,zcons,ind;
    isols:=convert~([RealDomain:-solve(eq,vars,explicit)],list);
    icons:=findSolutionDomain~(isols);
    # 对于单变量约束的情况，尝试进行补全
    zcons:=select(isNonZeroCon,`union`(icons[]));
    if numelems(zcons)=1 then
        isols:={isols[],convert~([RealDomain:-solve([eq[],indets(zcons,name)[]],vars,explicit)],list)[]};
        isols:=[isols[]];
        isols:=SolveTools[SortByComplexity](isols);
        icons:=findSolutionDomain~(isols);
    end if;
    # 删除对c有约束的解
    ind:=map(k->`if`(chkCCons(icons[k]),NULL,k),[seq(1..numelems(isols))]);
    isols:=isols[ind];
    icons:=icons[ind];
    return isols,icons;
end proc:

# 检查是存在仅和c有关的约束
chkCCons:=proc(con)
    return ormap(x->type(indets(x,name),set(specindex(c))),con);
end proc:

# 选择最优代表元
selectRep:=proc(s::InvSol)
    local tlist;
    tlist:=TsolsList(s:-tsols);
    printTsolsList(tlist);
    s:-tsolsList:=tlist;
    s:-rep:=tlist:-reps[1];
    s:-tInd:=tlist:-torders[1];
    s:-tsol:=tlist:-tsols[1];
    s:-tcon:=tlist:-tcons[1];
    s:-state:=5;
    resolve(s);
end proc:

$endif