# Berechnung und Projektion der Dynamikgleichungen
# Einleitung
# Berechnung und Projektion der Dynamikgleichungen
#
# 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
# Autor
# Tim Job (Studienarbeit bei Moritz Schappler), 2018-12
# Moritz Schappler, moritz.schappler@imes.uni-hannover.de
# (C) Institut für Mechatronische Systeme, Universität Hannover
# Sources
# [Abdellatif2007] Modellierung, Identifikation und robuste Regelung von Robotern mit parallelkinematischen Strukturen
# [Job2018_S759] Job, T. (Studienarbeit; Betreuer Moritz Schappler): Implementierung einer strukturunabhängigen Dynamikmodellierung für parallelkinematische Maschinen (2018)
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
with(StringTools): # Für Zeitausgabe
;
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster).
#codegen_act := true: # noch nicht implementiert
codegen_debug := false:
codegen_opt := 2:
codeexport_invdyn := true:
codeexport_actcoord := false: # Generierung der Dynamik in Antriebskoordinaten nicht standardmäßig (hoher Rechenaufwand)
;
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", leg_name):
# Kennung des Parametersatzes, für den die Dynamikfunktionen erstellt werden sollen. Muss im Repo und in der mpl-Datei auf 1 gelassen werden, da die folgende Zeile mit einem Skript verarbeitet wird.
codegen_dynpar := 1:
# Ergebnisse der zusätzlichen Definitionen für parallele Roboter laden
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/para_definitions", robot_name):
# Ergebnisse der Plattform-Dynamik laden (aus robot_para_plattform_rotmat_dynamics.mw)

read "../robot_codegen_definitions/robot_env_par":
dynamicsfile := sprintf("../codeexport/%s/tmp/floatb_platform_dynamic_maple.m", robot_name):
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:
# Neu-Definition der geladenen Variablen
MME:=MME:
cvecE:=cvecE:
gE:=gE:
tauE:=tauE:
H:=H:
dH:=dH:

# Ergebnisse der Kinematik für parallelen Roboter laden

read "../robot_codegen_definitions/robot_env_par":
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:
read "../robot_codegen_definitions/robot_env_par": # Nochmal laden, um Standard-Einstellungen überschreiben zu können.
# Neu-Definition der von dieser Datei gelesenen Variablen, damit sie im Workspace erscheinen
pivotMat := pivotMat:
pivotMatMas := pivotMatMas:
Jinv := Jinv:
JBinv_i := JBinv_i:
U_i := U_i:

# 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")): # TODO: Muss hier die Winkelkonvention eingesetzt werden? Wird das hier gebraucht?
# TODO: Euler-Funktion mit "parse"-Befehl hier definieren
# 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:
# Startzeit messen zur Beurteilung der Zeitdauer einzelner Schritte
st := time():
# Physikalische Parameter der durch Koppelgelenke bewegten Körper zu Null setzen.
NQ := NQ - (NQJ-NQJ_parallel):
for i from NQJ_parallel+1 to NQJ do
	XXC||i := 0:
	XYC||i := 0:
	XZC||i := 0:
	YYC||i := 0:
	YZC||i := 0:
	ZZC||i := 0:
	XX||i := 0:
	XY||i := 0:
	XZ||i := 0:
	YY||i := 0:
	YZ||i := 0:
	ZZ||i := 0:
	SX||i := 0:
	SY||i := 0:
	SZ||i := 0:
	MX||i := 0:
	MY||i := 0:
	MZ||i := 0:
	M||i := 0:
end do:
# Ergebnisse G-Vektor der Beinkette  laden.
# Die Rotation der Basis wird nur in der Jacobi-Matrix der inverse Kinematik berücksichtigt. Deshalb muss der Gravitationsvektor ebenfalls an die Rotation angepasst werden.

g1 := gtmp1:
g2 := gtmp2:
g3 := gtmp3:

dynamicsfile_leg := sprintf("../codeexport/%s/tmp/gravload_par%d_maple.m", leg_name, codegen_dynpar):
if FileTools[Exists](dynamicsfile_leg) then
  read dynamicsfile_leg:
else
  printf("%s. Beinketten-Dynamik (g) 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:
G := simplify2(Matrix(taug_s(7..NQ,1))):
unassign('g1','g2','g3'):
g := <g1;g2;g3>:
Rmat := Transpose(parse(sprintf("eul%s2r",angleConvLeg))(frame_A_i(1..3,1))):
gtmp1 := (Rmat.g)(1):
gtmp2 := (Rmat.g)(2):
gtmp3 := (Rmat.g)(3):
G := G:

# Ergebnisse C-Vektor der Beinkette laden
dynamicsfile_leg := sprintf("../codeexport/%s/tmp/coriolisvec_par%d_maple.m", leg_name, codegen_dynpar):
if FileTools[Exists](dynamicsfile_leg) then
  read dynamicsfile_leg:
else
  printf("%s. Beinketten-Dynamik (c) 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:
Cvec := simplify2(Matrix(tauCC_s(7..NQ,1))):
Cvec := Cvec:

# Ergebnisse M-Matrix der Beinkette laden

dynamicsfile_leg := sprintf("../codeexport/%s/tmp/inertia_par%d_maple.m", leg_name, codegen_dynpar):
if FileTools[Exists](dynamicsfile_leg) then
  read dynamicsfile_leg:
else
  printf("%s. Beinketten-Dynamik (M) 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:
MM := simplify(MM_s(7..NQ,7..NQ)):
MM := simplify(MM):
MME := simplify(MME):
# Ausdruck für Gravitationsterme der Plattform nochmal vereinfachen.
gE := simplify2(gE):
# Ergebnisse der Kinematik für parallen Roboter laden (wurde oben schon gemacht.
#read sprintf("../codeexport/%s/tmp/kinematics_%s_platform_maple.m", robot_name, base_method_name):
printf("%s. Alle Daten geladen. Generiere Dynamik für PKM %s mit Parametersatz %d\n", FormatTime("%Y-%m-%d %H:%M:%S"), robot_name, codegen_dynpar, base_method_name):

# Berechne Dynamik-Matrizen für alle Beine
# Dupliziere alle berechneten Matrizen. i steht für den Index des jeweiligen Beines

for i to N_LEGS do
  MM||i := Copy(MM):
  Cvec||i := Copy(Cvec):
  G||i := Copy(G):
end do:
# Substituiere in jeder Matrix den Winkel Alpha (Verdrehung in der Basis) und die Gelenkkoordinaten und -geschwindigkeiten
for k from 1 by 1 to N_LEGS do
  	for i to NQJ_parallel do
  		for l to 3 do
  	 		Cvec||k(i,1):=subs({frame_A_i(l,1)=frame_A_i(l,k)},Cvec||k(i,1)):
  	  		G||k(i,1):=subs({frame_A_i(l,1)=frame_A_i(l,k)},G||k(i,1)):
  		end do:
    		for m to NQJ_parallel do #alpha
      		n := (m + (k-1)*NQJ_parallel):
     		Cvec||k(i,1):=subs({qJD||m||s=qJ||D||n||s,qJ||m||s=qJ||n||s},Cvec||k(i,1)):
      		G||k(i,1):=subs({qJ||m||s=qJ||n||s},G||k(i,1)):
    		end do:
    		for j to NQJ_parallel do
    			for l to 3 do
    	 			MM||k(i,j):=subs({rame_A_i(l,1)=frame_A_i(l,k)},MM||k(i,j)):
    	 		end do:
      		for m to NQJ_parallel do #alpha
        			n := m + (k-1)*NQJ_parallel:
        			MM||k(i,j):=subs({qJ||m||s=qJ||n||s},MM||k(i,j)):
      		end do:
    		end do:
  	end do:
end do:

# Berechnung, Projektion und Addition der Dynamikgleichungen
# Berechnung der Kräfte/Momente an den Gelenken der jeweiligen Beine und Projektion auf EE-Plattform
# Abdellatif2007 S.38 (3.27); [Job2018_S759], S. 29

for i to N_LEGS do

  Jtmp := Multiply(Transpose(U_i(..,..,i)),Transpose(JBinv_i(..,..,i))):
  qDtmp := Multiply(JBinv_i(..,..,i),U_i(..,..,i).H.xED_s):
  A||i := simplify(Multiply(JBinv_i(..,..,i),JBD_i(..,..,i))):
  B||i := Multiply(-MM||i,Multiply(A||i,qDtmp)):

  # [Job2018_S759], Term in der Summe in Gl. (3.50)
  MMs||i := Jtmp . MM||i . Transpose(Jtmp) . H:
  # [Job2018_S759], Term in der Summe in Gl. (3.51)
  cvecs||i := Jtmp.MM||i.JBinv_i(..,..,i).(U_i(..,..,i).dH + UD_i(..,..,i).H).xED_s + Jtmp.B||i + Jtmp.Cvec||i:
  # [Job2018_S759], Term in der Summe in Gl. (3.52)
  gvecs||i := Jtmp.G||i:

  tau||i := Jtmp.MM||i.JBinv_i(..,..,i).(U_i(..,..,i).H.xEDD_s+U_i(..,..,i).dH.xED_s+UD_i(..,..,i).H.xED_s) + Multiply(Jtmp,(B||i+Cvec||i+G||i)):

  taus||i := MMs||i.xEDD_s + cvecs||i + gvecs||i:
end do:

# Abdellatif2007 S.40 (3.33); [Job2018_S759], (3.49)
# Aufsummieren aller Kräfte, projiziert auf EE-Plattform
Tmp := 0:
for i to N_LEGS do
  Tmp := Tmp + tau||i:
end do:
# Addiere Inverse Dynamik der Plattform
tauGes := Tmp + tauE:
# Aufsummieren aller Massenmatrizen, projiziert auf EE-Plattform
# [Job2018_S759], (3.50)
Tmp := 0:
for i to N_LEGS do
  Tmp := Tmp + MMs||i:
end do:
# Addiere Massenmatrix der Plattform
MMGes := Tmp + MME:
# Aufsummieren aller Coriolisvektoren, projiziert auf EE-Plattform
# [Job2018_S759], (3.51)
Tmp := 0:
for i to N_LEGS do
  Tmp := Tmp + cvecs||i:
end do:
# Addiere Coriolisvektor der Plattform
cvecGes := Tmp + cvecE:
# Aufsummieren aller Gravitiationsvektoren, projiziert auf EE-Plattform
# [Job2018_S759], (3.52)
Tmp := 0:
for i to N_LEGS do
  Tmp := Tmp + gvecs||i:
end do:
# Addiere Gravitiationsvektor der Plattform
gGes := Tmp - gE:
#tauGes := MMGes.xEDD_s + cvecGes + gGes:
# Replace Joint Velocities
# Substituiere die Gelenkgeschwindigkeiten über H-, Ui- und JBi-Matrix mit EE-Geschwindikeiten
Tmp := 0:
for i to N_LEGS do
  Tmp := Multiply(H,xED_s):
  Tmp := Multiply(U_i(..,..,i),Tmp):
  z||i := Multiply(JBinv_i(..,..,i),Tmp):
end do:
for i to 6 do
  for j to N_LEGS do
    for l to NQJ_parallel do
      tauGes(i,1) := subs({qJD_i_s(l,j)=z||j(l)},tauGes(i,1)):
      cvecGes(i,1) := subs({qJD_i_s(l,j)=z||j(l)},cvecGes(i,1)):
      gGes(i,1) := subs({qJD_i_s(l,j)=z||j(l)},gGes(i,1)):
      for k to 6 do
        MMGes(i,k) := subs({qJD_i_s(l,j)=z||j(l)},MMGes(i,k)):
      end do:
    end do:
  end do:
end do:
# Export
# Wähle die Einträge aus Dynamikgleichungen, die für Freiheitsgrade des Roboters relevant sind.
# (über die Auswahl-Matrix "pivotMat").
#Jtestinv := Matrix(6,6,symbol=Jentry):
#Jtest := MatrixInverse(Jtestinv):
#Jtest := simplify(Jtest):
#for i to RowDimension(Jtest) do
#  for j to ColumnDimension(Jtest) do
#    for k to RowDimension(Jtest) do
#      for l to ColumnDimension(Jtest) do
#        Jtest(i,j) := subs(Jentry[k,l]=simplify(Jinv(k,l)),Jtest(i,j)):
#      end do:
#    end do:
#  end do:
#end do:
# Dynamik-Terme in Plattform-Koordinaten
tau_x := pivotMat.tauGes:
MMGes_x := pivotMat.MMGes.Transpose(pivotMatMas):
cvecGes_x := pivotMat.cvecGes:
gGes_x := pivotMat.gGes:
# Maple-Export (zur eventuellen späteren Verarbeitung in Maple)
save tau_x,     sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_par%d_maple.m",      robot_name, codegen_dynpar):
save MMGes_x,   sprintf("../codeexport/%s/tmp/inertia_para_plfcoord_par%d_maple.m",     robot_name, codegen_dynpar):
save cvecGes_x, sprintf("../codeexport/%s/tmp/coriolisvec_para_plfcoord_par%d_maple.m", robot_name, codegen_dynpar):
save gGes_x,    sprintf("../codeexport/%s/tmp/gravvec_para_plfcoord_par%d_maple.m",     robot_name, codegen_dynpar):
# Dynamik in Antriebs-Koordinaten umrechnen. Nur machen, wenn die Jacobi-Matrix einfach genug ist. Sonst ist die symbolische Invertierung zu teuer und sollte numerisch gemacht werden
# [Job2018_S759], S. 30; Gl. 3.53, 3.54
if RowDimension(Jinv) < 5 and codeexport_actcoord then
  printf("%s. Beginn der Matrix-Invertierung.  CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
  J:=MatrixInverse(Jinv): # TODO: Matrix-Invertierung in eigenem Skript (bei der Kinematik; dort mit Platzhalter-Matrix invertieren)
  save J, sprintf("../codeexport/%s/tmp/jacobian_maple.m", robot_name): # TODO: Besseren Namen wählen und dies im Kinematik-Skript machen.
  printf("%s. Matrix-Invertierung beendet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
  # J:=simplify(J):
  # printf("%s. Optimierung beendet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
  tau_qa     := Transpose(J) . tau_x:
  MMGes_qa   := Transpose(J) . MMGes_x:
  cvecGes_qa := Transpose(J) . cvecGes_x:
  gGes_qa    := Transpose(J) . gGes_x:
  # Maple-Export (zur eventuellen späteren Verarbeitung in Maple)
  save tau_qa,     sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_par%d_maple.m",      robot_name, codegen_dynpar):
  save MMGes_qa,   sprintf("../codeexport/%s/tmp/inertia_para_actcoord_par%d_maple.m",     robot_name, codegen_dynpar):
  save cvecGes_qa, sprintf("../codeexport/%s/tmp/coriolisvec_para_actcoord_par%d_maple.m", robot_name, codegen_dynpar):
  save gGes_qa,    sprintf("../codeexport/%s/tmp/gravvec_para_actcoord_par%d_maple.m",     robot_name, codegen_dynpar):
end if:
printf("%s. Speicherung der Dynamik-Terme in symbolischer Form beendet. Starte Code-Export in Matlab\n", FormatTime("%Y-%m-%d %H:%M:%S")):
# Matlab Export
if codeexport_invdyn then
  printf("%s. Beginne Code-Export Inverse Dynamik in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(tau_x,     sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Massenmatrix in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(MMGes_x,   sprintf("../codeexport/%s/tmp/inertia_para_plfcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Coriolis-Vektor in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(cvecGes_x, sprintf("../codeexport/%s/tmp/coriolisvec_para_plfcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Gravitations-Vektor in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(gGes_x,    sprintf("../codeexport/%s/tmp/gravvec_para_plfcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
end if:
if codeexport_invdyn and RowDimension(Jinv) < 5 and codeexport_actcoord then
  printf("%s. Beginne Code-Export Inverse Dynamik in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(tau_qa,     sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Massenmatrix in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(MMGes_qa,   sprintf("../codeexport/%s/tmp/inertia_para_actcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Coriolis-Vektor in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(cvecGes_qa, sprintf("../codeexport/%s/tmp/coriolisvec_para_actcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Gravitations-Vektor in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(gGes_qa,    sprintf("../codeexport/%s/tmp/gravvec_para_actcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
end if:


#
# These procedures come from the source code of GFun.
#
getname:=proc(yofz::function(name), y, z)
  y:=op(0,yofz);
  if type(y,'procedure') then error `not an unassigned name`,y fi;
  z:=op(yofz)
end proc:

#
# returns the smallest i such that u(n+i) appears in a recurrence
#
minindex := proc(rec,u,n)
  min(op(map(op,indets(rec,'specfunc'('linear'(n),u)))))-n
end proc:

#
# returns the largest i such that u(n+i) appears in a recurrence
#
maxindex := proc(rec,u,n)
  max(op(map(op,indets(rec,'specfunc'('linear'(n),u)))))-n
end proc:

#
# A recurrence of the form a(n+d) = p(n)/q(n) a(n) is represented through a record:
# OneTermRecurrence : record(order, numerator, denominator)
#
`type/OneTermRecurrence` := 'record(order, numerator, denominator)':

#
#checkOneTermRecurrence
# Input: a recurrence rec (either with or without initial conditions).
#          If it has initial conditions, they are ignored.
#        a(n): the name of the sequence and the name of the variable.
#
# Output:
# This procedure checks that rec is a recurrence of the form a(n+d) = p(n)/q(n) a(n)
# If the check succeeds, it returns the corresponding record. If it fails, an error is
# returned.
#
checkOneTermRecurrence := proc(rec, aofn)::OneTermRecurrence;
  local r, d, a, n, term1, term2, res;

  getname(aofn, a, n):
  if type(rec, 'set') then
    r:=select(has, rec, n);
    if nops(r)>1
      then error `invalid recurrence`, rec
    fi:
    if nops(r)=0
      then error "%1 does not appear in the recurrence", n
    fi:
    r := op(r):
  else r:=rec:
  fi:
  if type(r,'`=`')
    then r:=op(1,r)-op(2,r)
  fi:
  if indets(r,'specfunc'('anything',a)) <> indets(r,'specfunc'('linear'(n),a))
    then error "the recurrence contains elements that are not linear in %1", n
  fi:
  if nops(r) <> 2
    then error "the recurrence contains %1 terms (expected 2)", nops(r)
  fi:
  r := subs(n=n-minindex(r, a, n), r):
  d := maxindex(r, a, n):

  term1 := select(has, r, a(n)):
  term2 := select(has, r, a(n+d)):

  res := factor( -(term1/a(n)) / (term2/a(n+d)) ):

  Record( 'order'=d, 'numerator' = numer(res), 'denominator' = denom(res) )
end proc:

#
# my_factors factorizes p the same way as factors(p) would do except that the constant part is computed
# differently. We assume here that p has integer coefficients, and we want to factorize it over polynomials
# with integer coefficients. my_factors ensures that the factors have integer coefficients.
#
my_factors := proc(p)
  local L, c, fact, i, my_c, my_fact, q:
  L := factors(p):
  c := L[1]: fact := L[2]:
  my_c := c: my_fact := []:
  for i from 1 to nops(fact) do
    q := denom(fact[i][1]):
    my_fact := [ op(my_fact), [ fact[i][1]*q, fact[i][2] ] ]:
    my_c := my_c / (q^fact[i][2]):
  od:
  [ my_c, my_fact]:
end proc:

#
# This procedure decomposes a one-term recurrence with the following form:
# a(n+d) = c * s1(n)/s1(n+d) * s2(n+d)/s2(n) * p(n)/q(n) * a(n)
#
# Known issue: this procedure assumes that the only variables involved are n and x with their usual meaning.
#
decomposeOneTermRecurrence := proc(formalRec::OneTermRecurrence, res_cste, res_s1, res_s2, res_p, res_q)
  local p, q, cste, s1, s2, d, L, i, tmp, exponent, r, polyring;
  p := formalRec:-numerator:
  q := formalRec:-denominator:
  d := formalRec:-order:
  s1 := 1:
  L := op(2,my_factors(p)): # L contains the non trivial factors of p
  for i from 1 to nops(L) do
    tmp := L[i][1]: exponent := L[i][2]:
    r := gcd(tmp^exponent, subs(n=n-d, q)):
    p := quo(p,r,n): q := quo(q, subs(n=n+d, r),n): s1 := s1 * r:
  od:

  s2 := 1:
  L := op(2,my_factors(p)): # L contains the *remaining* non trivial factors of p
  for i from 1 to nops(L) do
    tmp := L[i][1]: exponent := L[i][2]:
    r := gcd(tmp^exponent, subs(n=n+d, q)):
    p := quo(p, r, n): q := quo(q, subs(n=n-d, r), n): s2 := s2 * r:
  od:

  # Finally we look for the constant part (with respect to n) of p/q
  cste := op(1, my_factors(p))/op(1, my_factors(q)):
  p := p/op(1, my_factors(p)): q := q/op(1, my_factors(q)):
  polyring := RegularChains[PolynomialRing]([n,x]):
  L := op(2, my_factors(p)):
  for i from 1 to nops(L) do
    if RegularChains[MainVariable](L[i][1], polyring) = x
      then cste := cste * L[i][1]^L[i][2]: p := quo(p,L[i][1]^L[i][2],x):
    fi:
  od:
  L := op(2, my_factors(q)):
  for i from 1 to nops(L) do
    if RegularChains[MainVariable](L[i][1], polyring) = x
      then cste := cste / L[i][1]^L[i][2]: q := quo(q,L[i][1]^L[i][2],x):
    fi:
  od:

  res_cste := cste;
  res_s1 := s1;
  res_s2 := s2;
  res_p := simplify(p);
  res_q := simplify(q);
end proc:

#
#coeffrecToTermsrec
# Input: a linear recurrence rec (either with or without initial conditions).
#           a(n): the name of the sequence and the name of the variable.
#           x: a value or symbolic name
#
# Output:
# The recurrence satisfied by a(n)*x^n. Note that this recurrence is also denoted by a(n).
# If initial conditions were provided, corresponding initial conditions are computed.
#
coeffrecToTermsrec := proc(rec, aofn, x)
  local a,n,L,r,cond,d,i,tmp,c,res;
  getname(aofn, a, n):
  if type(rec, 'set') then
    L := selectremove(has, rec, n):
    r := L[1]:
    if nops(r)>1
      then error `invalid recurrence`, rec
    fi:
    if nops(r)=0
      then error "%1 does not appear in the recurrence", n
    fi:
    r := op(r):
    cond := L[2]:
  else r := rec:
  fi:
  d := maxindex(r, a, n):
  L := indets(r,'specfunc'('linear'(n),a)):
  if indets(r,'specfunc'('anything',a)) <> L
    then error "the recurrence contains elements that are not linear in %1", n
  fi:
  L := map(op, L):
  for i from 1 to nops(L) do
    r := subs(a(op(i,L))=a(op(i,L))*x^(d-op(i,L)+n), r):
  od:
  if cond<>'cond' then
    c := {}:
    for i from 1 to nops(cond) do
      tmp := op(i, cond): # tmp should have the form 'a(k) = cste'
      if not type(tmp,'`=`') then error "Invalid initial condition: %1", tmp: fi:
      L := selectremove(has, {op(tmp)}, a):
      if (nops(L[1]) <> 1) or (nops(L[2])<>1)
        then error "Invalid initial condition: %1", tmp:
      fi:
      tmp := op(1, L[1]): # tmp has the form 'a(k)'
      c := {op(c), tmp = op(1, L[2])*x^op(tmp)}:
    od:
    res := {r, op(c)}:
  else res := r:
  fi:
  res:
end proc:

#
# This procedure removes the conditions of the form a(k)=0 from the initial conditions of rec
# It returns a list L = [L1, L2, ...] where Li = [k, expr] representing the condition a(k)=expr.
# Moreover, it asserts that the Li are ordered by increasing k.
#
removeTrivialConditions := proc(rec, aofn)
  local a,n,i,L,tmp,c,cond,k:
  getname(aofn, a, n):
  if not type(rec, 'set') then
    error "%1 is not a recurrence with initial conditions", rec
  else
    L := selectremove(has, rec, n):
    cond := L[2]:
    if nops(cond)=0
      then error "%1 does not contain initial conditions", rec
    fi:
  fi:
  c := []:
  for i from 1 to nops(cond) do
    tmp := op(i, cond): # tmp should have the form 'a(k) = cste'
    if not type(tmp,'`=`') then error "Invalid initial condition: %1", tmp: fi:
    L := selectremove(has, {op(tmp)}, a):
    if (nops(L[1]) <> 1) or (nops(L[2])<>1)
      then error "Invalid initial condition: %1", tmp:
    fi:
    if op(1, L[2])<>0 then c := [op(c), [op(op(1, L[1])), op(1, L[2])]]: fi:
  od:
  # We check that the conditions are ordered by increasing k.
  if (nops(c)=0) then return c: fi:
  k := c[1][1]:
  for i from 2 to nops(c) do
    if (c[i][1]<=k)
    then error "Unexpected error in removeTrivialConditions: the conditions are not correctly ordered (%1)\n", c
    else k := c[i][1]
    fi:
  od:
  c:
end proc:

#
# findFixpointOfDifferences: takes a set L of integer and returns the smallest set S
# containing L and such that for each i, S[i]-S[i-1] \in S
findFixpointOfDifferences := proc(L)
  local res, i:
  res := L:
  for i from 2 to nops(L) do
    res := { op(res), L[i]-L[i-1] }:
  od:
  if (res=L) then return res else return findFixpointOfDifferences(res) fi:
end proc:

#
# error_counter functions allows one to follow the accumulation of errors in each variable.
#   an error_counter is a list of the form [[var1, c1], [var2, c2], ... ]
#   where the vari are variable names and the ci indicate how many approximation errors
#   are accumulated in vari.
#

#
# This procedure initializes the counter associated with variable var to 1 (and creates it if needed.)
# It returns an up-to-date error_counter.
init_error_counter := proc (var, error_counter)
  local i, res:
  res  := error_counter:
  for i from 1 to nops(res) do
    if (res[i][1]=var)
    then res[i][2] := 1:
         return res:
    fi
  od:
  res := [op(res), [var, 1]]:
end:

#
# This procedure adds a given number to the counter associated with variable var.
# It returns an up-to-date error_counter.
add_to_error_counter := proc (var, n, error_counter)
  local i, res:
  res  := error_counter:
  for i from 1 to nops(res) do
    if (res[i][1]=var)
    then res[i][2] := res[i][2]+n:
         return res:
    fi
  od:
  res := [op(res), [var, n]]:
end proc:

#
# This procedure sets the value of the counter associated with variable var.
# It returns an up-to-date error_counter.
set_error_counter := proc(var, n, error_counter)
  local i,err:
  err  := error_counter:
  for i from 1 to nops(err) do
    if (err[i][1]=var)
    then err[i][2] := n:
         return err:
    fi
  od:
  err := [op(err), [var, n]]:
end proc:

#
# This procedure initializes the counter associated to the multiplication of var2 and var3,
# putting the result in variable var1.
# It returns an up-to-date error_counter.
error_counter_of_a_multiplication := proc (var1, var2, var3, error_counter)
  local i, res, c2, c3:
  c2 := 0: c3 := 0:
  for i from 1 to nops(error_counter) do
    if (error_counter[i][1]=var2) then c2 := error_counter[i][2] fi:
    if (error_counter[i][1]=var3) then c3 := error_counter[i][2] fi:
    if (error_counter[i][1]=var1)
    then
      res := [ op(error_counter[1..i-1]), op(error_counter[i+1..nops(error_counter)]) ]
    fi:
  od:
  if (res = 'res') then res := error_counter fi:
  res := [op(res), [var1, c2+c3+1]]:
end:

#
# Copies the error counter of var2 into var1
error_counter_on_copy := proc(var1, var2, error_counter)
  local i, err, c2:
  c2 := 0:
  for i from 1 to nops(error_counter) do
    if (error_counter[i][1] = var2) then c2 := error_counter[i][2] fi:
    if (error_counter[i][1] = var1)
    then
      err := [ op(error_counter[1..i-1]), op(error_counter[i+1..nops(error_counter)]) ]
    fi:
  od:
  if (err = 'err') then err := error_counter fi:
  if (c2 <> 0) then err := [op(res), [var1, c2]] fi:
end proc:

#
# Returns the value of the error counter associated to a variable
find_in_error_counter := proc(var, error_counter)
  local i:
  for i from 1 to nops(error_counter) do
    if (error_counter[i][1] = var) then return error_counter[i][2] fi:
  od:
  return 0:
end proc:

#
# generate_multiply_rational(fd, var1, var2, r, error_counter, indent) generates code for performing
# var1 = var2*r in MPFR
#   fd is the file descriptor in which the code shall be produced.
#   var1 and var2 are strings representing variable names. r is a Maple rational number.
#   error_counter is an error_counter (as described above).
#   indent is an optional argument. It is a string used to correctly indent the code. It is prefixed to any
#   generated line. Hence, if indent="  ", the generated code will be indented by 2 spaces.
# An up-to-date error_counter is returned.
generate_multiply_rational := proc(fd, var1, var2, r, error_counter, indent:="")
  local p,q,err:
  err := error_counter:
  if (whattype(r)<>'fraction') and (whattype(r)<>'integer')
  then error "generate_multiply_rational used with non rational number %1", r: fi:
  if (abs(r)=1)
  then
    if (var1=var2)
    then
      if (r<>1) then fprintf(fd, "%sMPFR_CHANGE_SIGN (%s);\n", indent, var1) fi:
      return err:
    else
      if (r=1)
        then fprintf(fd, "%smpfr_set (%s, %s, MPFR_RNDN);\n", indent, var1, var2):
        else fprintf(fd, "%smpfr_neg (%s, %s, MPFR_RNDN);\n", indent, var1, var2):
      fi:
      return error_counter_on_copy(var1, var2, err):
    fi
  fi:
  # Now, r is a rational number different from 1.
  p := numer(r): q := denom(r):
  if (abs(p)<>1)
  then
    fprintf(fd, "%smpfr_mul_si (%s, %s, %d, MPFR_RNDN);\n", indent, var1, var2, p):
    err := error_counter_of_a_multiplication(var1, var2, "", err):
    if(q<>1)
    then
      fprintf(fd, "%smpfr_div_si (%s, %s, %d, MPFR_RNDN);\n", indent, var1, var1, q):
      err := error_counter_of_a_multiplication(var1, var1, "", err):
    fi:
  else
    fprintf(fd, "%smpfr_div_si (%s, %s, %d, MPFR_RNDN);\n", indent, var1, var2, p*q):
    err := error_counter_of_a_multiplication(var1, var2, "", err):
  fi:
  return err:
end proc:

#
# generate_multiply_poly is the same as generate_multiply_rational but when r is a rational fraction.
# The fraction r must have the form p/q where p and q are polynomials with integer coefficients.
# Moreover, the gcd of the coefficients of p must be 1. Idem for q.
# The procedure returned a list [m, d, err] where m is the set of indices k such that
# a mpfr_mul_sik function is needed and idem for d with mpfr_div_sik.
# err is an up-to-date error counter.
generate_multiply_poly := proc(fd, var1, var2, r, error_counter, indent:="")
  local p,q,Lp,Lq,n,i,j,var, required_mulsi, required_divsi, err:
  err := error_counter:
  required_mulsi := {}:
  required_divsi := {}:
  p := numer(r): q := denom(r):
  Lp := my_factors(p): Lq := my_factors(q):
  if (Lp[1] <> 1)
    then error "generate_multiply_poly: an integer can be factored out of %1", p:
  fi:
  if (Lq[1] <> 1)
    then error "generate_multiply_poly: an integer can be factored out of %1", q:
  fi:
  Lp := Lp[2]: Lq := Lq[2]:
  var := var2:
  if (nops(Lp) <> 0)
  then
    n := 0:
    for i from 1 to nops(Lp) do n := n + Lp[i][2] od:
    if (n=1)
    then
      fprintf(fd, "%smpfr_mul_si (%s, %s", indent, var1, var):
    else
      required_mulsi := { op(required_mulsi), n }:
      fprintf(fd, "%smpfr_mul_si%d (%s, %s", indent, n, var1, var):
    fi:
    for i from 1 to nops(Lp) do
      for j from 1 to Lp[i][2] do
        fprintf(fd, ", %a", Lp[i][1]):
      od:
    od:
    fprintf(fd, ", MPFR_RNDN);\n"):
    err := set_error_counter(var1, n+find_in_error_counter(var, err) , err):
    var := var1:
  fi:
  if (nops(Lq) <> 0)
  then
    n := 0:
    for i from 1 to nops(Lq) do n := n + Lq[i][2] od:
    if (n=1)
    then
      fprintf(fd, "%smpfr_div_si (%s, %s", indent, var1, var):
    else
      required_divsi := { op(required_divsi), n }:
      fprintf(fd, "%smpfr_div_si%d (%s, %s", indent, n, var1, var)
    fi:
    for i from 1 to nops(Lq) do
      for j from 1 to Lq[i][2] do
        fprintf(fd, ", %a", Lq[i][1])
      od:
    od:
    fprintf(fd, ", MPFR_RNDN);\n"):
    err := set_error_counter(var1, n+find_in_error_counter(var, err) , err):
    var := var1:
  fi:
  if (var1 <> var) then
    fprintf(fd, "%smpfr_set (%s, %s, MPFR_RNDN);\n", indent, var1, var):
    err := set_error_counter(var1, find_in_error_counter(var, err) , err):
  fi:
  return [required_mulsi, required_divsi, err]:
end proc:

#
# This function generates the code of a procedure mpfr_mul_uin or mpfr_div_uin
#
generate_muldivsin := proc(op, n)
  local i, var:
  if ((op <> "mul") and (op <> "div"))
    then error "Invalid argument to generate_muldivuin (%1). Must be 'mul' or 'div'", op
  fi:
  if (whattype(n) <> 'integer')
  then error "Invalid argument to generate_muldivuin (%1). Must be an integer.", n
  fi:

  if (op="mul") then var := "MUL" else var := "DIV" fi:

  printf("__MPFR_DECLSPEC void mpfr_div_si%d _MPFR_PROTO((mpfr_ptr, mpfr_srcptr,\n", n):
  for i from n to 2 by -2 do
    printf("                                               long int, long int,\n"):
  od:
  if (i=1)
  then
    printf("                                               long int, mpfr_rnd_t));\n"):
  else
    printf("                                               mpfr_rnd_t));\n")
  fi:

  printf("\n\n\n"):
  printf("void\n"):
  printf("mpfr_%s_si%d (mpfr_ptr y, mpfr_srcptr x,\n", op, n):
  for i from n to 2 by -2 do
    printf("              long int v%d, long int v%d,\n", n-i+1, n-i+2):
  od:
  if (i=1)
  then
    printf("              long int v%d, mpfr_rnd_t mode)\n", n):
  else
    printf("              mpfr_rnd_t mode)\n")
  fi:
  printf("{\n"):
  printf("  long int acc = v1;\n"):
  printf("  mpfr_set (y, x, mode);\n"):
   for i from 2 to n do
    printf("  MPFR_ACC_OR_%s (v%d);\n", var, i):
  od:
  printf("  mpfr_%s_si (y, y, acc, mode);\n", op):
  printf("}\n"):
  return:
end proc:


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


var nTimes : int := 0;
print "How many times? ";
read nTimes;
var x : int;
for x in 0..nTimes-1 do
    print x;
    print " : Hello, World!\n";
end for;
assert (x = nTimes);


with(LinearAlgebra): with(ArrayTools): with(QuantumChemistry):
loaddata := readdata(cat(cdir,"_temp.rdm"), 8):
Flatten := proc(x) local n, a, i, j, k, l; `local`(a, n, i, j, k, l); `description`("convert chemists to numpy and flatten an array to form a matrix"); n := Size(x); a := Matrix(1 .. n[1]*n[2], 1 .. n[3]*n[4], datatype = float[8]); for i to round(n[1]) do for j to round(n[2]) do for k to round(n[3]) do for l to round(n[4]) do a[(i - 1)*n[1] + j, (k - 1)*n[3] + l] := x[i, j, k, l]; end do; end do; end do; end do; return a; end proc:
New := Array(1 .. 4, 1 .. 4, 1 .. 4, 1 .. 4, datatype = float[8]):
for i in loaddata[3 .. ()] do
    New[round(i[1]), round(i[2]), round(i[3]), round(i[4])] := i[5]:
end do:
pure := Purify2RDM(New, spin_free = false, electron_number = 4, conv_tol = 0.10000000):
ExportMatrix(cat(cdir, "_temp_purified.csv"), Flatten(pure[rdm2])):


 func $liori32 (
  var %i i32, var %j i32
  ) i32 {
   return (
     lior i32(dread i32 %i, dread i32 %j))}

 func $liori64 (
  var %i i64, var %j i64
  ) i32 {
   return (
     lior 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


# Extract a domain from an expression
export Extract := module ()
    export ModuleApply := proc(e, kb, $) :: [ HDomain, anything ];
        local b, eb, s, es;
        b, eb := op(Bound(e));
        s, es := op(Shape(eb, kb ));
        [ DOMAIN(b, s), es ];
    end proc;

    # Extract a domain bound from an expression
    # This pops off the integration constructors recursively, keeping
    # track of the bounds in a KB (which literally become the DBound).
    export Bound := module ()
        export ModuleApply := proc(e, $) :: [ DomBound, anything ];
            local arg, vars, kb;
            arg, vars := do_extract(e)[];
            vars := Domain:-Bound:-withVarsIxs(DBound(vars));
            [ vars , arg ];
        end proc;

        local do_extract_arg := proc(kind, arg_)
            local x0, x, vars, arg := arg_, rng;
            x0  := ExtBound[kind]:-ExtractVar(_rest);   # variable name
            rng := ExtBound[kind]:-ExtractRange(_rest); # variable range
            x   := DInto(x0, rng, kind);                # the variable spec
            arg, vars := do_extract(arg)[];
            [ arg, [ op(vars), x ] ];
        end proc;

        local do_extract := proc(arg, $)
            local sub, prod, svars;
            if arg :: `*` then
                sub := map(do_extract, [op(arg)]);
                prod, svars := selectremove(x->op(2,x)=[],sub);
                if nops(svars) = 1 then
                    [ `*`(op([1,1],svars),op(map2(op,1,prod)))
                    , op([1,2], svars) ];
                else
                    [ arg, [] ];
                end if;
            elif Domain:-Has:-Bound(arg) then
                do_extract_arg(op(0,arg), op(arg));
            else
                [ arg, [] ]
            end if;
        end proc;
    end module;

    # Extract a domain shape from an expression
    # This extracts the domain shape from individual multiplicands of
    # expressions, and multiplies the subexpressions back together.
    # essentially this assumes a distributive law (of domain shapes over
    # products)
    export Shape := module ()
        export ModuleApply := proc(e) :: [ anything, anything ];
            local ixs, w, e1;
            ixs := [indices(ExtShape, 'nolist')];
            w, e1 := do_gets(ixs, true, e) [];
            if not ('no_simpl' in {_rest}) then
                w := simpl_shape(w);
            end if;
            [ w, e1 ];
        end proc;

        local do_get := proc(f, f_ty, e, $)
            local sub, inds, rest;
            if e::`*` then
              sub := map(x->do_get(f, f_ty,x), [op(e)]);
              [ `And`(op(map2(op,1,sub))), `*`(op(map2(op,2,sub))) ]
            elif e::`^` then
              inds, rest := do_get(f, f_ty, op(1,e)) [] ;
              [ inds, subsop(1=rest, e) ]
            elif e:: f_ty then
              f(e,z->ModuleApply(z,'no_simpl'))
            else
              [ true, e ]
            end if
        end proc;

        # apply a list of extractors, in order, until all fail to produce
        # any output .
        local do_gets := proc(todo::list, w, e, $)
            local t0, ts, w1, e1;
            if nops(todo) = 0 then
                [ w, e ]
            else
                t0 := op(1, todo);
                ts := op(subsop(1=NULL, todo));
                w1, e1 := do_get(ExtShape[t0]:-MakeCtx
                                ,ExtShape[t0]:-MapleType
                                ,e) [] ;
                ts := `if`(is(w1), [ts], [ts, t0]);
                do_gets( ts, bool_And(w1, w), e1 );
            end if;
        end proc;

        # todo: simplify the shape
        local simpl_shape := proc(e0,$)
            local e := Domain:-simpl_relation(e0);
            e := subsindets(e, specfunc(`Or`) , x->DSum(op(x)));
            e := subsindets(e, specfunc(`And`), x->DConstrain(op(x)));
            e;
        end proc;
    end module;
end module;


with(Threads):
id:=Create(int(sin(x)^x,x),res);
res;
Wait(id);
res;


$ifndef _INVSOL_
$define _INVSOL_

InvSol:=module()
    option object;
    export
            # 公用变量
            As::static,     # 每个生成元的伴随矩阵
            A::static,      # 总的伴随矩阵
            nvars::static,  # 总的变量个数
            # 实例变量
            state,          # 状态代码，主要为人工干预提供入口
                            # 0：还未生成不变量方程
                            # 1：不变量方程求解失败
                            # 2：取特解失败
                            # 3：变换方程求解失败
                            # 4：等待选择最优代表元
                            # 5：求解完成
            oeq::static,    # 偏微分方程组，
                            # 迭代过程中oeq保持不变
                            # 通过变量代换来获取实际的oeq
            Deltas:=[],     # 不变量
            orders:=[],     # 不变量的阶数
            ieq:=[],        # 不变量方程程组，按不变量排序
            ieqCode:=0,     # 不变量方程的编号
            isols:=[],      # 不变量方程组的解
            icons:=[],      # 不变量方程组对应的条件
            isolInd:=1,     # 通解的下标
            useBranch:=false,# 使用分支求解则假设每个特解都是不等价的
            rsols:=[],      # 不变量方程的特解
            tsols:=[],      # TeqSol对象list
            tsolsList,      # TsolsList 对象
            rep,            # 代表元
            # 最优变换方程的解
            teq,
            tInd,
            tsol,
            tcon,

            vars,           # 需要求解的系数
            # 条件相关
            # 不变量方程+附加约束+展示约束，描述了代表元所代表的元素的范围
            # 后期只维护以下变量，而不修改icons,tcons等原始信息
            addcons:={},    # 附加约束，能够参与计算，其中，
                            # + 等式是对不变量方程的扩充，例如a[1]=0
                            # + 不等式式对取值范围的描述，并能够进行处理，例如a[1]>0
            discons:={},    # 展示约束，仅用于展示信息，不能参与计算，包括
                            # + a[1]*a[3]>0 这种不能处理的多变量约束
            # 导出函数
            getSubs::static,     # 在求解新的不变量时代入的条件
            updateVars::static,  # 更新求解变量
            getRealOeq::static,  # 获取代换后的oeq
            getZeroCons::static, # 获取为零约束
            addZeroCons::static, # 添加为零约束
            getIsolCons::static, # 返回通解的约束
            displayIeq::static,  # 显示不变量方程
            printSol::static,
            getDisplayIeq::static,
            getDisplayCons::static,
            uniqueKey::static,
            ModulePrint::static; # 显示函数

    getIsolCons:=proc(s::InvSol)
        return s:-addcons union s:-discons union s:-icons[s:-isolInd];
    end proc:

    ModulePrint:=proc(s::InvSol)
        if s:-state<5 then
            return sprintf("unsolved %d",s:-state);
        else
            return s:-rep;
        end if;
    end proc:

    uniqueKey:=proc(s::InvSol)
        return ModulePrint(s);
    end proc:

    printSol:=proc(s::InvSol)
        if s:-state<5 then
            printf("求解失败 %d",s:-state);
        else
            printf("--------------------------------\n");
            print(s:-rep);
            print(getDisplayCons(s));
            if s:-isols<>[] then
                print(s:-isols[s:-isolInd]);
                print(s:-tsol);
                print(s:-tcon);
            end if;
        end if;
    end proc:

    getDisplayIeq:=proc(s::InvSol)
        return [seq(Delta[i]=rhs(s:-ieq[i]),i=1..numelems(s:-ieq))];
    end proc:

    getDisplayCons:=proc(s::InvSol)
        local res:={};
        if s:-isols<>[] then
            res:=res union s:-icons[s:-isolInd];
        end if;
        res:=res union s:-addcons union s:-discons;
        res:=res minus convert(s:-ieq,set);
        return [getDisplayIeq(s)[],res[]];
    end proc:

    getSubs:=proc(s::InvSol)
        local r;
        r:=getZeroCons(s);
        r:=[r[]];
        if s:-isols<>[] then
            r:=[r[],s:-isols[s:-isolInd][]];
        end if;
        return remove(x->evalb(lhs(x)=rhs(x)),{r[]});
    end proc:

    updateVars:=proc(s::InvSol)
        s:-vars:=s:-vars minus indets(lhs~(getSubs(s)),name);
    end proc:

    getRealOeq:=proc(s::InvSol)
        local oeq:=s:-oeq;
        updateVars(s);
        oeq:=PDETools:-dsubs(phi(seq(a[i],i=1..s:-nvars))=phi(s:-vars[]),oeq);
        oeq:=eval(subs(getSubs(s)[],oeq)) minus {0};
        return oeq;
    end proc:

    getZeroCons:=proc(s::InvSol)
        return select(type,s:-addcons,equation);
    end proc:

    addZeroCons:=proc(s::InvSol,c::set)
        s:-addcons:=s:-addcons union c;
        s:-vars:=s:-vars minus indets(c,name);
    end proc:

    displayIeq:=proc(s::InvSol)
        local n;
        n:=numelems(s:-ieq);
        flog[1]([seq(Delta[i]=rhs(s:-ieq[i]),i=1..n)]);
        flog[1]([seq(Delta[i]=s:-Deltas[i],i=1..n)]);
    end proc:

end module:

$endif


#struct aa{
#  double b;
#  float *x;
#};

#struct bb {
#  struct aa b1;
#  struct aa *pa;
#  double x1[10];
#  double dx;
#};

#typedef struct ss {
#  int f1;
#  char f2:3;
#  struct aa a3;
#  struct aa *pa1;
#  char f3:5;
#  struct bb *b3;
#  struct bb *b2;
#  struct aa xt[23];
#  struct aa(* funcc)(int, int, double);
#}SS;

type $unname1 <struct {@b f64,
                       @x <* f32>}>
type $unname2 <struct {@b1 <struct {@b f64,
                                    @x <* f32>}>,
                       @pa <* <struct {@b f64,
                                       @x <* f32>}>>,
                       @x1 <[10] f64>,
                       @dx f64}>
type $SS      <struct {@f1 i32,
                       @f2:3 i8,
                       @a3 <structincomplete {@b f64,
                                    @x <*f32>}>,
                       @pa1 <* <struct {@b f64,
                                       @x <* f32> } >>,
                       @f3:5 i32,
                       @b3 <* <struct {@b1 <struct {@b f64,
                                                    @x <* f32>}>,
                                       @pa <* <struct {@b f64,
                                                       @x <* f32>}>>,
                                       @x1 <[10] f64>,
                                       @dx f64}>>,
                       @b2 <* $unname2>,
                       @xt <[23] <struct {@b f64,
                                          @a <*f32>}>>,
                       @funcc <func(i32, i32, f64)<struct {@b f64,
                                                           @x <* f32>}>>}>

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


 func $cvtu64tof64 (
  var %i u64
  ) f64 {
   return (
     cvt f64 u64(dread u64 %i))}
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


$ifndef _TEST_H_
$define _TEST_H_
sign:=proc(x)
    if x>0 then
        1
    elif x<0 then
        -1
    else
        0
    end if;
end proc:
$endif


$ifndef _CLASSIFY_HOLDER_
$define _CLASSIFY_HOLDER_

$include "Utils.mpl"
ClassifyHolder:=module()
    local   ieqCode,sols,unsolvedSols;
    export  reset,      # 重置状态
            addSol,     # 新增解
            getSols,    # 获取解
            addUnsolvedSol,
            getUnsolvedSols,
            getIeqCode; # 获取不变量方程的编号

    reset:=proc()
        ieqCode:=0;
        sols:={};
        unsolvedSols:={};
        return;
    end proc:

    addSol:=proc(s::InvSol)
        flogf[1]("添加代表元");
        flog[1](s:-rep);
        sols:=sols union {s};
        return;
    end proc:

    getSols:=proc()
        local res:=sols;
        res:=uniqueObj(res,InvSol:-uniqueKey);
        res:=sort(res,'key'=(x->x:-ieqCode));
        return res;
    end proc:

    getIeqCode:=proc()
        ieqCode:=ieqCode+1;
        return ieqCode;
    end proc:

    addUnsolvedSol:=proc(s)
        unsolvedSols:=unsolvedSols union {s};
        return;
    end proc:

    getUnsolvedSols:=proc()
        return unsolvedSols;
    end proc:

end module:

$endif


# Step 2 of 3: computer algebra

improve := proc(lo :: LO(name, anything), {_ctx :: t_kb := empty}, opts := [], $)
local r, `&context`;
  userinfo(5, improve, "input: ", print(lo &context _ctx));
  _Env_HakaruSolve := true;
  r:= LO(op(1,lo), reduce(op(2,lo), op(1,lo), _ctx, opts));
  userinfo(5, improve, "output: ", print(r));
  r
end proc;

# Walk through integrals and simplify, recursing through grammar
# h - name of the linear operator above us
# kb - domain information
reduce := proc(ee, h :: name, kb :: t_kb, opts := [], $)
  local e, elim, subintegral, w, ww, x, c, kb1, with_kb1, dom_specw, dom_specb
       , body, dom_spec, ed, mkDom, vars, rr
       , do_domain := evalb( not ( "no_domain" in {op(opts)} ) ) ;
  e := ee;

  if do_domain then
    rr := reduce_Integrals(e, h, kb, opts);
    if rr <> FAIL then return rr end if;
  end if;
  if e :: 'applyintegrand(anything, anything)' then
    map(simplify_assuming, e, kb)
  elif e :: `+` then
    map(reduce, e, h, kb, opts)
  elif e :: `*` then
    (subintegral, w) := selectremove(depends, e, h);
    if subintegral :: `*` then error "Nonlinear integral %1", e end if;
    subintegral := convert(reduce(subintegral, h, kb, opts), 'list', `*`);
    (subintegral, ww) := selectremove(depends, subintegral, h);
    simplify_factor_assuming(`*`(w, op(ww)), kb)
      * `*`(op(subintegral));
  elif e :: Or(Partition,t_pw) then
    if e :: t_pw then e := PWToPartition(e); end if;
    e := Partition:-Simpl(e);
    e := kb_Partition(e, kb, simplify_assuming,
                      ((rhs, kb) -> %reduce(rhs, h, kb, opts)));
    e := eval(e, %reduce=reduce);
    # big hammer: simplify knows about bound variables, amongst many
    # other things
    Testzero := x -> evalb(simplify(x) = 0);
    e := Partition:-Simpl(e);
    if ee::t_pw and e :: Partition then
      e := Partition:-PartitionToPW(e);
    end if;
    e;
  elif e :: t_case then
    subsop(2=map(proc(b :: Branch(anything, anything))
                   eval(subsop(2='reduce'(op(2,b),x,c,opts),b),
                        {x=h, c=kb})
                 end proc,
                 op(2,e)),
           e);
  elif e :: 'Context(anything, anything)' then
    kb1 := assert(op(1,e), kb);
    # A contradictory `Context' implies anything, so produce 'anything'
    # In particular, 42 :: t_Hakaru = false, so a term under a false
    # assumption should never be inspected in any way.
    if kb1 :: t_not_a_kb then
        return 42
    end if;
    applyop(reduce, 2, e, h, kb1, opts);
  elif e :: 'toLO:-integrate(anything, Integrand(name, anything), list)' then
    x := gensym(op([2,1],e));
    # If we had HType information for op(1,e),
    # then we could use it to tell kb about x.
    subsop(2=Integrand(x, reduce(subs(op([2,1],e)=x, op([2,2],e)), h, kb, opts)), e)
  else
    simplify_assuming(e, kb)
  end if;
end proc;

# "Integrals" refers to any types of "integrals" understood by domain (Int,
# Sum currently)
reduce_Integrals := module()
  export ModuleApply;
  local
  # The callbacks passed by reduce_Integrals to Domain:-Reduce
    reduce_Integrals_body, reduce_Integrals_into
  # tries to evaluate a RootOf
  , try_eval_Root
  # tries to evaluate Int/Sum/Ints/Sums
  , elim_intsum;

  reduce_Integrals_body := proc(h,opts,x,kb1) reduce(x,h,kb1,opts) end proc;
  reduce_Integrals_into := proc(h,opts,kind,e,vn,vt,kb,$)
    local rr;
    rr := elim_intsum(Domain:-Apply:-do_mk(args[3..-1]), h, kb,opts);
    rr := subsindets(rr, specfunc(RootOf), x->try_eval_Root(x,a->a));
    return rr;
  end proc;

  ModuleApply := proc(expr, h, kb, opts, $)
    local rr;
    rr := Domain:-Reduce(expr, kb
      ,curry(reduce_Integrals_into,h,opts)
      ,curry(reduce_Integrals_body,h,opts)
      ,(_->:-DOM_FAIL));
    rr := kb_assuming_mb(Partition:-Simpl)(rr, kb, x->x);
    if has(rr, :-DOM_FAIL) then
      return FAIL;
    elif has(rr, FAIL) then
      error "Something strange happened in reduce_Integral(%a, %a, %a, %a)\n%a"
           , expr, kb, kb, opts, rr;
    end if;
    rr;
  end proc;

  try_eval_Root := proc(e0::specfunc(`RootOf`),on_fail := (_->FAIL), $)
    local ix,e := e0;
    try
      if nops(e)=2 or nops(e)=3
      and op(-1,e) :: `=`(identical(index),{specindex(real),nonnegint}) then
        ix := op([2,-1],e);
        if ix :: specindex(real) then ix := op(ix); end if;
        e := op(0,e)(op(1,e));
      else
        ix := NULL;
      end if;
      e := convert(e, 'radical', ix);
      if e :: specfunc(RootOf) then return on_fail(e) end if;
      return e;
    catch: return on_fail(e0); end try;
  end proc;

  # Try to find an eliminate (by evaluation, or simplification) integrals which
  # are free of `applyintegrand`s.
  elim_intsum := module ()
    export ModuleApply := proc(inert0, h :: name, kb :: t_kb, opts, $)
       local ex, e, inert := inert0;
       ex := extract_elim(inert, h, kb);
       e[0] := apply_elim(h, kb, ex);
       e[1] := check_elim(inert, e[0]);
       if e[1] = FAIL then inert
       else
         e[2] := reduce(e[1],h,kb,opts);
         if has(e[2], {csgn}) then
           WARNING("Throwing away an eliminated result result containing csgn (this "
                   "could be a bug!):\n%1\n(while running %2)", e[2], ex);
           inert;
         else e[2] end if;
       end if
    end proc;

    local known_tys := table([Int=int_assuming,Sum=sum_assuming,Ints=ints,Sums=sums]);
    local extract_elim := proc(e, h::name, kb::t_kb,$)
      local t, intapps, var, f, e_k, e_args, vs, blo, bhi;
      vs := {op(KB:-kb_to_variables(kb))};
      t := 'applyintegrand'('identical'(h), 'anything');
      intapps := indets(op(1,e), t);
      if intapps = {} then
        return FAIL;
      end if;
      e_k := op(0,e); e_args := op([2..-1],e);
      if Domain:-Has:-Bound(e) and assigned(known_tys[e_k]) then
        var := Domain:-ExtBound[e_k]:-ExtractVar(e_args);
        ASSERT(var::DomBoundVar);
        blo, bhi := Domain:-ExtBound[e_k]:-SplitRange
                    (Domain:-ExtBound[e_k]:-ExtractRange(e_args));
        if ormap(b->op(1,b) in map((q-> (q,-q)), vs) and op(2,b)::SymbolicInfinity
                ,[[blo,bhi],[bhi,blo]]) then
          return FAIL end if;
        if var :: list then var := op(1,var) end if;
        if not depends(intapps, var) then
          f := known_tys[e_k];
        else
          return FAIL;
        end if;
      end if;
      [ op(1,e), f, var, [e_args] ];
    end proc;

    local apply_elim := proc(h::name,kb::t_kb,todo::{list,identical(FAIL)})
      local body, f, var, rrest;
      if todo = FAIL then return FAIL; end if;
      body, f, var, rrest := op(todo);
      banish(body, h, kb, infinity, var,
             proc (kb1,g,$) do_elim_intsum(kb1, f, g, op(rrest)) end proc);
    end proc;

    local check_elim := proc(e, elim,$)
      if has(elim, {MeijerG, undefined, FAIL}) or e = elim or elim :: SymbolicInfinity then
        return FAIL;
      end if;
      return elim;
    end proc;

    local do_elim_intsum := proc(kb, f, ee, v::{name,name=anything})
      local w, e, x, g, t, r;
      w, e := op(Domain:-Extract:-Shape(ee));
      w := Domain:-Shape:-toConstraints(w);
      e := piecewise_And(w, e, 0);
      e := f(e,v,_rest,kb);
      x := `if`(v::name, v, lhs(v));
      g := '{sum, sum_assuming, sums}';
      if f in g then
        t := {'identical'(x),
              'identical'(x) = 'Not(range(Not({SymbolicInfinity, undefined})))'};
      else
        g := '{int, int_assuming, ints}';
        t := {'identical'(x),
              'identical'(x) = 'anything'};
        if not f in g then g := {f} end if;
      end if;
      for r in indets(e, 'specfunc'(g)) do
        if 1<nops(r) and op(2,r)::t then return FAIL end if
      end do;
      e
    end proc;
  end module; # elim
end module; # reduce_Integrals

int_assuming := proc(e, v::name=anything, kb::t_kb, $)
  simplify_factor_assuming('int'(e, v), kb);
end proc;

sum_assuming := proc(e, v::name=anything, kb::t_kb)
  simplify_factor_assuming('sum'(e, v), kb);
end proc;

# Int( .., var=var_ty ) == var &X var_ty
isBound_IntSum := kind -> module()
  option record;
  export MakeKB := (`if`(kind=Sum,KB:-genSummation,KB:-genLebesgue));
  export ExtractVar := (e->op(1,e));
  export ExtractRange := (e->op(2,e));
  export MakeRange := `..`;
  export SplitRange := (e->op(e));
  export Constrain := `if`(kind=Sum,`<=`,`<`);
  export DoMk := ((e,v,t)->kind(e,v=t));
  export Min := `min`; export Max := `max`;
  export VarType := 'name';
  export RangeType := 'range';
  export MapleType := 'And'('specfunc'(kind), 'anyfunc(anything,name=range)');
  export BoundType := `if`(kind=Sum,'integer','real');
  export RecogBound := `if`(kind=Sum,
            (proc(k,b)
               if   k = `<=` then (x->subsop(2=b,x))
               elif k = `>=` then (x->subsop(1=b,x))
               elif k = `<`  then (x->subsop(2=(b-1),x))
               elif k = `>`  then (x->subsop(1=b+1,x))
               end if;
             end proc),
            (proc(k,b)
               if   k in {`<=`,`<`} then (x->subsop(2=b,x))
               elif k in {`>=`,`>`} then (x->subsop(1=b,x))
               end if;
             end proc));
end module;

# Ints( .., var::name, var_ty::range, dims::list(name=range) ) ==
#        [ var   , map(lhs,dims) ] :: list(name)  &X
#        [ var_ty, map(rhs,dims) ] :: list(range)
isBound_IntsSums := kind -> module()
  option record;
  export MakeKB := proc(vars, lo, hi, kb, $)
    local var, dims, ty, rngs, x, kb1;
    var  := op(1, vars);
    rngs := zip(`..`,lo,hi);
    ty   := op(1, rngs);
    dims := subsop(1=NULL,zip(`=`,vars,rngs));
   x, kb1 := genType(var,
                      mk_HArray(`if`(kind=Ints,
                                     HReal(open_bounds(ty)),
                                     HInt(closed_bounds(ty))),
                                dims),kb);
    if nops(dims) > 0 then
      kb1 := assert(size(x)=op([-1,2,2],dims)-op([-1,2,1],dims)+1, kb1);
    end if;
    x, kb1;
  end proc;
  export ExtractVar   := ((v,t,d)->[v,map(lhs,d)[]]);
  export ExtractRange := ((v,t,d)->[t,map(rhs,d)[]]);
  export MakeRange    := ((a,b)->zip(`..`,a,b));
  export SplitRange   := (rs->(map(x->op(1,x),rs), map(x->op(2,x),rs)));
  export Constrain    := ((a,b)->zip(`if`(kind=Ints, `<`, `<=`),a,b)[]);
  export DoMk         := ((e,v,t)->kind( e,op(1,v),op(1,t), subsop(1=NULL,zip(`=`,v,t)) ));
  export Min          := ((a,b)->zip(`min`,a,b));
  export Max          := ((a,b)->zip(`max`,a,b));
  export VarType      := 'And(list(name),satisfies(x->x<>[]))';
  export RangeType    := 'And(list(range),satisfies(x->x<>[]))';
  export MapleType    := 'And'('specfunc'(kind),'anyfunc'('anything', 'name', 'range', 'list(name=range)'));
  export BoundType    := TopProp;
  export RecogBound   := (_->NULL);
end module;


interface(screenwidth=9999):
if not (NewSLO :: `module`) then
  WARNING("loading NewSLO failed");
  `quit`(3);
end if;

with(Hakaru):
with(NewSLO):

#####################################################################
#
# Dirichlet conjugacy tests
#
#####################################################################

(dice_index, dice_index_t) := Concrete("examples/dice_index.hk"):
dice_index := value(eval(dice_index)):

(gmm_gibbs, gmm_gibbs_t) := Concrete("examples/gmm_gibbs.hk"):
gmm_gibbs := value(eval(gmm_gibbs)):

(naive_bayes_gibbs, naive_bayes_gibbs_t) := Concrete("examples/naive_bayes/naive_bayes_gibbs.hk"):
naive_bayes_gibbs := value(eval(naive_bayes_gibbs)):

#####################################################################
TestEfficient( dice_index, dice_index_t, KB:-empty, label="dice index" ):
TestEfficient( gmm_gibbs, gmm_gibbs_t, KB:-empty, label="gmm gibbs" ):
TestEfficient( naive_bayes_gibbs, naive_bayes_gibbs_t, KB:-empty, label="naive bayes gibbs" ):


(*
    以代表元为核心的对象
    主要以con划分对象，isol和tsol仅做参考作用
    保证RepSol对象的所有属性的相关变量都是global的。
*)
$ifndef _REPSOL_
$define _REPSOL_

RepSol:=module()
    option object;
    local
            # 局部函数
            getDisplayDcon::static, # 合并显示成立条件
            apList::static,         # 拓展list
            rmlist::static;         # 去除一层嵌套list
    export
            # 导出变量
            rep,                    # 代表元
            dcon:=[],               # 不变量方程
            acon:=[],               # 附加约束
            isol:=[],               # 代表元的通解
            tsol:=[],               # 代表元通解和特解的转化
            osol:=[],               # 对应的InvSol对象
            sid:=1,                 # 选择的最简条件
            # 导出函数
            ## 设置相关
            appendSol::static,      # 扩充一个RepSol对象成立的条件
            getCon::static,         # 获取一个RepSol对象成立的条件
            sortCon::static,        # 对成立条件进行排序
            selectCon::static,      # 选择最简成立条件
            rmCon::static,          # 删除某个成立条件的一部分
            ## 输出相关
            printRep::static,       # 简要显示代表元和所有可能的成立条件
            fullPrintRep::static,   # 显示代表元和完整的成立条件以及对应的不变量方程和变换方程的解
            ModulePrint::static,    # 简要显示代表元
            uniqueAndSort::static;      # 删去重复条件

    # 用于拓展一个代表元对象所能代表的区域
    appendSol:=proc(r::RepSol,s::InvSol)
        local ieq,sieq,isol,icon,tsols,tcons,i,n;
        if not assigned(r:-rep) then
            r:-rep:=getRep(s);
        end if;
        ieq:=s:-ieq;
        sieq:={ieq[]};
        isol:=s:-isol;
        icon:=s:-icon;
        tsols:=rmlist(s:-tsol);
        tcons:=rmlist(s:-tcon);
        n:=numelems(tsols);
        for i from 1 to n do
            apList(r:-dcon,ieq);
            apList(r:-acon,classifySolve({icon[],tcons[i][]}) minus sieq);
            apList(r:-isol,isol);
            apList(r:-tsol,tsols[i]);
            apList(r:-osol,s);
        end do;
        return;
    end proc:

    # 拓展list
    apList:=proc(lst::evaln)
        lst:=[eval(lst)[],_rest];
        return;
    end proc:

    # 获取一个RepSol对象成立的条件
    getCon:=proc(r::RepSol)
        return zip((x,y)->[getDisplayDcon(x)[],y[]],r:-dcon,r:-acon);
    end proc:

    # 不变量方程的简化显示
    getDisplayDcon:=proc(dcon)
        local n;
        n:=numelems(dcon);
        return [seq(Delta[i]=rhs(dcon[i]),i=1..n)];
    end proc:

    # 去掉一层嵌套list
    rmlist:=proc(x)
        return map(y->y[],x);
    end proc:

    # 对成立条件进行排序
    sortCon:=proc(r::RepSol)
        local con,ind;
        con:=getCon(r);
        ind:=sortByComplexity(con,index);
        r:-dcon:=r:-dcon[ind];
        r:-acon:=r:-acon[ind];
        r:-isol:=r:-isol[ind];
        r:-tsol:=r:-tsol[ind];
        r:-osol:=r:-osol[ind];
    end proc:

    # 选择最简成立条件
    selectCon:=proc(r::RepSol,sid)
        r:-sid:=sid;
        return;
    end proc:

    # 删除某个成立条件的一部分
    rmCon:=proc(r::RepSol,id::posint,con::set)
        r:-acon[id]:=r:-acon[id] minus con;
    end proc:

    # 简要显示代表元和所有可能的成立条件
    printRep:=proc(r::RepSol)
        print(r:-rep);
        print~(getCon(r));
        return ;
    end proc:

    # 显示代表元和完整的成立条件以及对应的不变量方程和变换方程的解
    fullPrintRep:=proc(r::RepSol)
        local i,n,con;
        print(r:-rep);
        con:=getCon(r);
        n:=numelems(con);
        for i from 1 to n do
            printf("[%d]------------------------------------------------------",i);
            print(con[i]);
            print(r:-isol[i]);
            print(r:-tsol[i]);
        end do;
        return;
    end proc:

    # 删去重复条件
    uniqueAndSort:=proc(r::RepSol)
        local id,con,fun,ind;
        fun:=(x,y,z)->[x,y,z];
        con:=getCon(r);
        id:=fun~(con,r:-isol,r:-tsol);
        ind:=uniqueObj(id,key=(x->x),'index');
        r:-dcon:=r:-dcon[ind];
        r:-acon:=r:-acon[ind];
        r:-isol:=r:-isol[ind];
        r:-tsol:=r:-tsol[ind];
        r:-osol:=r:-osol[ind];
        sortCon(r);
        return r;
    end proc:

    # 简要显示代表元
    ModulePrint:=proc(r::RepSol)
        return r:-rep;
    end proc:
end module:

$endif


func $switchfunc ( var %n i32) i32 {
 var %alocal i32
 switch (dread i32 %n) @labdft {
   -3: goto @lab0
   1: goto @lab1
   9: goto @lab9 }
@lab0
 return (constval i32 -3)
@labdft
 return (constval i32 100)
@lab9
 return (constval i32 9)
@lab1
 return (constval i32 1) }
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


# typedef struct ss {
#   int f1;
#   char f2:3;
#   char f3:5;
# } SS;
# SS foo(SS x) {
#   x.f2 = 32;
#   return x;
# }
type $SS <struct {@f1 i32, @f2:3 i8, @f3:5 i8}>
func $foo (
	var %x <$SS>) i32 {
  dassign %x 2 ( constval i32 32 )
  return ( dread i32 %x 2 ) }
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


f1(){
	[x, y] = stack(), stack();
	x*7-y; // return value
}
f2()inline{
	[x, y] = stack(), stack();
	x*7-y; // return value
}
f3()inline, CRPL{swap 7 mul swap sub}
f4()CRPL{swap 7 mul swap sub}

main()inline{
	ShowTraceLog();
	ClearTraceLog();

	Trace("f1 " $ f1(1, 2));
	Trace("f2 " $ f2(1, 2));
	Trace("f3 " $ f3(1, 2));
	Trace("f4 " $ f4(1, 2));
}


with(StringTools):
DiagLatex := proc(spart, size:=0)
	local phis, thetas, phithetas, zeds, Lambda, string, cercle, triangle, triangle_cercle, boite, part, endline, k;
	phis:= spart[2];
	phis:= map( x-> [x,"c"], phis);
	thetas:= spart[3];
	thetas:= map(x-> [x,"b"], thetas);
	phithetas:= spart[1];
	phithetas:= map(x-> [x,"d"], phithetas);
	zeds:= spart[4];
	zeds:= map(x-> [x,"a"], zeds);
	Lambda:= sort([op(phithetas),op(phis), op(thetas), op(zeds)]);

	string:="";

	cercle:= "\\yC";
	triangle:= "\\yT";
	triangle_cercle:= "\\yTC";
	boite:= "\\,";
	for k from 1 to nops(Lambda) do
		part:= Lambda[k];
		print(Lambda);
		print(part);
		if k = 1 then endline:=""; else endline:="\\\\ \n"; end if:
		if part[2] = "c" then
			string:= cat(cat(Repeat(cat(boite, "& "), part[1]), triangle, endline), string);
		elif part[2] = "b" then
			string:= cat(cat(Repeat(cat(boite, "& "), part[1]), cercle, endline), string);
		elif part[2] = "a" then
			string:= cat(cat(Repeat(cat(boite, "& "), part[1]-1), boite, endline), string);
		elif part[2] = "d" then
			string:= cat(cat(Repeat(cat(boite, "& "), part[1]), triangle_cercle, endline), string);
		end if;
	end do:
	if size = 0 then
		string:= cat("{\\, \\superYsmall{", string, "}}");
	elif size = 1 then
		string:= cat("{\\, \\superY{", string, "}}");
	elif size = 2 then
		string:= cat("\\superRusse{{\\superY{", string, "}}}");
	end if;
	return string;
end proc:

superLatex := proc(expr,initial_term:=0, wipe:=0)
	local sparts, astring, spart, spart_string, fd, basetype, terms, thecoeff, latexsparts, latex_terms, k, ou;
	if wipe <> 0 then
		fd:= fopen("out.txt", WRITE);
		fprintf(fd, "");
		fclose(fd);
	else
		fd:=fopen("out.txt", APPEND);
		fprintf(fd,"new \n \\begin{gather*} \n");
		fclose(fd);
	end if;
	basetype:= "m";
	terms:= [op(indets(expr, 'superindexed'))];
	terms:= sort(terms, list_sort);
	#print(terms);
	sparts:= map(x-> [op(x)], terms);
	#print(sparts);
	thecoeff:= map(x-> coeff(expr, x), terms);
	#print(thecoeff);
	#latexcoeff:= map(x-> latex(x), thecoeff);

	#Activate following line for diagrams
	#latexsparts:= map(x-> DiagLatex(x,0), sparts);
	#Activate following line for sparts
	latexsparts:= map(x-> SpartLatex(x),sparts);
	latex_terms:= [];
	if initial_term <> 0 then
		fd:=fopen("out.txt", APPEND);
		fprintf(fd,cat(initial_term[1],"\\,",basetype,"_",DiagLatex(initial_term[2], 0),"=","\n+"));
		fclose(fd);
	end if;
	for k from 1 to nops(sparts) do
		fd:= fopen("out.txt", APPEND);
		fprintf(fd, "\t");
		fclose(fd);
		if whattype(thecoeff[k]) = `+` then
			fd:= fopen("out.txt", APPEND);
			fprintf(fd, "(");
			fclose(fd);
		end if;

		latex(thecoeff[k],"out.txt",'append');

		if whattype(thecoeff[k]) = `+` then
			fd:= fopen("out.txt", APPEND);
			fprintf(fd, ")");
			fclose(fd);
		end if;
		fd:= fopen("out.txt", APPEND);
		fprintf(fd, cat(basetype,"_",latexsparts[k],"\n+"));
		fclose(fd);
	end do:
		fd:=fopen("out.txt", APPEND);
		fprintf(fd,"\\end{gather*} \n");
		fclose(fd);
end proc:

SpartLatex:= proc(spart)
	local pts, phis, thetas, zeds, pt_string, phi_string, theta_string, zed_string, SpartString;
	pts:= spart[1];
	phis:= spart[2];
	thetas:= spart[3];
	zeds:= spart[4];

	pt_string:= convert(pts, string);
	phi_string:= convert(phis, string);
	theta_string:= convert(thetas, string);
	zed_string:= convert(zeds, string);

	pt_string:= Substitute(pt_string, "[", "(");
	pt_string:= Substitute(pt_string, "]", ";\\,");
	phi_string:= Substitute(phi_string, "[", "");
	phi_string:= Substitute(phi_string, "]", ";\\,");
	theta_string:= Substitute(theta_string, "[", "");
	theta_string:= Substitute(theta_string, "]", ";\\,");
	zed_string:= Substitute(zed_string, "[", "");
	zed_string:= Substitute(zed_string, "]", ")");

	SpartString:= cat(pt_string, phi_string, theta_string, zed_string);
	SpartString:= cat("{",SpartString, "}");
	return SpartString;
end proc:


`is_element/tree_FFbar` := (N::posint) -> (A::set) -> (TT) -> proc(Q)
 local TT1,B,C,P,T,U;
 global reason;

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

 P := sort(map(sort,map(op,{indices(Q)})));
 TT1 := select(T -> nops(T) > 1,TT);

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

 C := children_map(A)(TT);

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

  for U in C[T] do
   if nops(U) > 1 and not(`is_element/SCP2`(N)(T,U)([Q[T],Q[U]])) then
    reason := [convert(procname,string),"(Q[T],Q[U]) is not in SCP2(N)(T,U)",eval(Q[T]),eval(Q[U]),N,T,U,reason];
    return false;
   fi;
  od;
 od;

 return true;
end;

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

`random_element/tree_FFbar` := (N::posint) -> (A::set) -> (TT) -> proc()
 `mu/tree_Fbar/tree_FFbar`(N)(A)(TT)(`random_element/tree_Fbar`(N)(A)(TT)());
end;

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

`is_equal/tree_FFbar` := (N::posint) -> (A::set) -> (TT) -> proc(P,Q)
 local T,TT1;
 global reason;

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

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

 return true;
end;

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

`is_leq/tree_FFbar` := (N::posint) -> (A::set) -> (TT) -> proc(P,Q)
 local T,TT1;
 global reason;

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

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

 return true;
end;

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

`is_interior/tree_FFbar` := (N::posint) -> (A::set) -> (TT) -> proc(Q)
 return `is_element/ICP`(N)(A)(Q[A]);
end;

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

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

 Q := table();
 TT1 := select(T -> nops(T) > 1,TT);
 for T in TT1 do
  Q[T] := `top/autorel`(T) intersect Q0;
 od;

 return eval(Q);
end:

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

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

 return Q[A];
end;

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

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

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

 Q1 := table():
 for T in TT1 do Q1[T] := eval(Q[T]); od;

 return eval(Q1);
end:

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

`ext/tree_FFbar/FFbar` := (N::posint) -> (A::set) -> (TT) -> proc(Q)
 local TT1,UU,TU,T,U,m,Q1,i;

 TT1 := select(T -> nops(T) > 1,TT);
 UU := `list_elements/big_subsets`(A);

 Q1 := table():

 for U in UU do
  TU := select(T -> U minus T = {},TT1);
  m := min(op(map(nops,TU)));
  TU := select(U -> nops(U) = m,TU);
  T := TU[1];
  Q1[U] := [seq(`top/autorel`(U) intersect Q[T][i],i=1..N)];
  if not(`is_element/SCP`(N)(U)(Q1[U])) then
   return FAIL;
  fi;
 od;

 return eval(Q1);
end:

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

`C/tree_FFbar` := (N::posint) -> (A::set) -> (TT) -> proc(Q)
 local L,TT1,T,G,i,ab;
 L := NULL;
 TT1 := select(T -> nops(T) > 1,TT);
 for T in TT1 do
  G := `gamma/SCP`(N)(T)(Q[T]);
  L := L,seq(seq([i,op(ab)],ab in G[i]),i=1..N);
 od;
 return {L};
end:

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

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

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

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

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

 Q := table();

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

 return eval(Q);
end;

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

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

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

 x := table();

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

 return eval(x);
end;


# Class: RealAlgebraicNumber
#
# Description:
#  Implementation of real algebraic numbers together with their comparison.
#  This implementation is inspired by the implementation in CGAL 4.7.
#
# 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.
#
module RealAlgebraicNumber()
  option object;
  (* Univariete polynomaial *)
  local poly::polynom;
  (* Lower bound of the range in which exists only one real root of poly. *)
  local a::rational;
  (* Upper bound of the range in which exists only one real root of poly. *)
  local b::rational;
  (* Real algebraic number is rational when a = b and sign of poly at a/b is 0. *)
  local isRational_;
  (* Note that isolating interval has to be open iff a real algebraic number is not rational and
  closed, a = b, otherwise.*)

# Method: ModuleCopy
#   Standard constructor / copy constructor
#
# Parameters:
#   self::RealAlgebraicNumber     - a new object to be constructed
#   proto::RealAlgebraicNumber    - a prototype object from which self is derived
#   poly::polynom                 - a univariate polynomial
#   a::rational                   - a lower bound of the range in which exists only one real root of poly
#   b::rational                   - an upper bound of the range in which exists only one real root of poly
#
# Output:
#   An object of type RealAlgebraicNumber.
#
# Exceptions:
#  "Invalid range. A range is valid when: a <= b."
#  "Degree of %1 is invalid."
#
  export ModuleCopy::static := proc( self::RealAlgebraicNumber,
                                     proto::RealAlgebraicNumber,
                                     poly::polynom,
                                     a::rational,
                                     b::rational, $ )
    local signAtA, signAtB;
    if _passed = 2 then
      self:-poly := proto:-poly;
      self:-a := proto:-a;
      self:-b := proto:-b;
      self:-isRational_ := proto:-isRational_;
    else
      if upperbound( indets( poly ) ) > 1 then
        error "%1 is not univariate!", poly;
      end if;
      if a > b then
        error "Invalid range. A range is valid when: a <= b.";
      end if;
      if gcd( poly, diff( poly, op( indets( poly ) ) ) ) <> 1 then
        error "Polynomial: %1 is not square-free.", poly;
      end if;
      self:-poly := poly;
      if degree(poly) >= 1 then
        signAtA := signum( eval( poly, indets( poly )[1] = a ) );
        signAtB := signum( eval( poly, indets( poly )[1] = b ) );
        self:-a := a;
        self:-b := b;
        self:-isRational_ := evalb( self:-a = self:-b and signAtA = 0 );
        if signAtA = 0 and signAtB <> 0 then
          WARNING("Incorrect interval. Sign of univariate polynomial on one side of the interval"
          " evaluated to zero but not on the another. Interval fixed.");
          self:-b := self:-a;
          self:-isRational_ := true:
        elif signAtB = 0 and signAtA <> 0 then
          WARNING("Incorrect interval. Sign of univariate polynomial on one side of the interval"
          " evaluated to zero but not on the another. Interval fixed.");
          self:-a := self:-b;
          self:-isRational_ := true:
        elif signAtA = signAtB and self:-a <> self:-b then
          error "Interval incorrect! No root in the interval: (%1, %2), for %3 .", self:-a, self:-b,
          self:-poly;
        fi:
      elif degree( poly ) = 0 then
        self:- denom( poly ) * 'a'  - numer( poly );
        self:-a := poly;
        self:-b := poly;
        self:-isRational_ := true;
      else
        error "Degree of %1 is invalid.", poly;
      end if;
    end if;
    return self;
  end proc:

# Method: ModulePrint
#   Standard printout of an object of type RealAlgebraicNumber.
#
# Parameters:
#   self::RealAlgebraicNumber      - a real algebraic number
#
  export ModulePrint::static := proc( self::RealAlgebraicNumber )
    if(self:-a = self:-b) then
        nprintf( "( %a, [%a, %a] )", self:-poly, self:-a, self:-b );
    else
        nprintf( "( %a, ]%a, %a[ )", self:-poly, self:-a, self:-b );
    end if;
  end proc;

# Method: ModuleApply
#   Define standard constructor.
#
  export ModuleApply::static := proc()
   Object(RealAlgebraicNumber, args)
  end proc;

# Method: ModuleDeconstruct
#   Provides information how to recreate an object after being serialized.
#
# Parameters:
#   self::RealAlgebraicNumber        - a real algebraic number
#
  export ModuleDeconstruct := proc( self::RealAlgebraicNumber )
    ('RealAlgebraicNumber')(self:-poly, self:-a, self:-b)
  end proc;

# Method: GetPolynomial
#   A getter method to access the univariate polynomial of RealAlgebraicNumber.
#
# Parameters:
#   self::RealAlgebraicNumber      - a real algebraic number
#
# Output:
#   Univariate polynomial stored in self.
#
  export GetPolynomial::static := proc( self::RealAlgebraicNumber )
    return self:-poly;
  end proc:

# Method: GetInterval
#   A getter method to access the range isolating a root of univariate polynomial.
#
# Parameters:
#   self::RealAlgebraicNumber      - a real algebraic number
#
# Output:
#   The range isolating a root of univariate polynomial -- self:-poly.
#   Upper and lower bounds of a range are rationals. When lower = upper
#   then a real algebraic number is rational.
#
  export GetInterval::static := proc( self::RealAlgebraicNumber )
    return [ self:-a, self:-b ];
  end proc:

# Method: IsRational
#   A method to check if a real algebraic number is rational.
#
# Parameters:
#   self::RealAlgebraicNumber      - a real algebraic number
#
# Output:
#   True when a real algebraic number is rational, false
#   otherwise. A real algebraic number is meant as rational when a = b and
#   when sign of poly at a/b is zero.
#
  export IsRational::static := proc( self::RealAlgebraicNumber )
    return self:-isRational_;
  end proc:

# Method: CompareRational
#   A method used to compare a real algebraic number with a
#   rational number.
#
# Parameters:
#   self::RealAlgebraicNumber      - a real algebraic number
#   m::rational                    - a rational number
#
# Output:
#   -1 when a real algebraic number is smaller than a rational
#    number, 0 when they are equal and 1 when a real algebraic
#    number is bigger than a rational.
#
  local CompareRational::static := proc( self::RealAlgebraicNumber, m::rational )
    local refined:
    refined := StrongRefineAt(self,m);
    if evalb( refined:-a < m ) then
      return -1;
    elif evalb( refined:-a > m ) then
      return 1;
    elif evalb( signum( eval( refined:-poly, op( indets( refined:-poly ) ) = m ) ) = 0 ) then
      return 0;
    end if;
  end proc:

# Method: RefineAt
#   A method used to refine a real algebraic number using a rational
#   number for adaptation of a range isolating a root of poly.
#
# Parameters:
#   self::RealAlgebraicNumber      - a real algebraic number
#   m::rational                    - a rational number
#
# Output:
#   A RealAlgebraicNumber obtaind from self refined at m.
#
# Comment:
#   Not that the type can change to rational.
#
  local RefineAt::static := proc( self::RealAlgebraicNumber, m::rational )
    local signAtM, f::polynom, g::polynom;
    local var := op( indets( self:-poly ) );
    if self:-isRational_ or m <= self:-a or self:-b <= m then
      return self;
    end if;
    signAtM := signum( eval( self:-poly, var = m ) );
    if evalb( signAtM = 0 ) then
     g := denom( m ) * var  - numer( m );
     return Object( RealAlgebraicNumber, g, m, m );
    elif evalb( signum( eval( self:-poly, var = self:-a ) ) = signAtM ) then
      return Object( RealAlgebraicNumber, self:-poly, m, self:-b );
    elif evalb( signum( eval( self:-poly, var = self:-b ) ) = signAtM ) then
      return Object( RealAlgebraicNumber, self:-poly, self:-a, m );
    else
      return self;
    end if;
  end proc:

# Method: BisectRange
#   A method used to compare a real algebraic number with a
#   rational number.
#
# Parameters:
#   self::RealAlgebraicNumber      - a real algebraic number
#
# Output:
#   Refine an isolating range at ( self:-a + self:-b ) / 2
#
  local BisectRange::static := proc( self::RealAlgebraicNumber )
    return RefineAt( self, ( self:-a + self:-b ) / 2 );
  end proc:

# Method: StrongRefineAt
#   A method used to refine a real algebraic number using a rational
#   number for adaptation of a range isolating a root of poly.
#
# Parameters:
#   self::RealAlgebraicNumber      - a real algebraic number
#   m::rational                    - a rational number
#
  local StrongRefineAt::static := proc( self::RealAlgebraicNumber, m::rational )
    local refined:
    if self:-isRational_ or signum( eval( self:-poly, indets( self:-poly )[1] = m ) ) = 0 then
      return self;
    fi:
    refined := self;
    while refined:-a <= m and m <= refined:-b do
      refined := BisectRange(refined);
    od:
    return refined:
  end proc:

  export DisjointRanges::static := proc(a::RealAlgebraicNumber, b::RealAlgebraicNumber)
    local ll := a, rr := b, i;

    (* No intersection.*)
    if evalb( ll:-b < rr:-a ) or evalb( ll:-a > rr:-b ) then
      return [a,b];
    fi:

    for i from 1 while 1 = 1 do
      ll := BisectRange( ll ):
      rr := BisectRange( rr ):

      (* No intersection.*)
      if evalb( ll:-b < rr:-a ) or evalb( ll:-a > rr:-b ) then
        return [ll,rr];
      fi:
    od:
  end proc:

# Method: Compare
#   A method used to compare two real algebraic numbers.
#
# Parameters:
#   l::RealAlgebraicNumber      - a real algebraic number
#   r::RealAlgebraicNumber      - a real algebraic number
#
# Output:
#   -1 when l is smaller than r, 0 when they are equal and 1 when l is bigger than r.
#
  export Compare::static := proc( l::RealAlgebraicNumber, r::RealAlgebraicNumber, $ )
    local i::integer, a::rational, b::rational, F1::polynom, F2::polynom, G::polynom;
    local ll::RealAlgebraicNumber, rr::RealAlgebraicNumber;

    if indets(l:-poly) <> indets(r:-poly) then
      error "Univariate polynomials have different variables: %1 and %2.", indets(l:-poly),
      indets(r:-poly);
    fi;

    if evalb( l:-poly = r:-poly and l:-a = r:-a and l:-b = r:-b ) then
        return 0;
    end if;

    (* When rationals *)
    if r:-isRational_ then
      return CompareRational( l, r:-a );
    elif l:-isRational_ then
      return -CompareRational( r, l:-a );
    end if;

    (* Check if there is no intersection of the ranges *)
    if evalb( l:-b < r:-a ) then
      return -1;
    elif evalb( l:-a > r:-b ) then
      return 1;
    end if:

    (* Get the intersecting interval *)
    if evalb( l:-a > r:-a ) then
      a := l:-a;
    else
      a := r:-a;
    end if;
    if evalb( l:-b < r:-b ) then
      b := l:-b;
    else
      b := r:-b;
    end if;

    (* refine at the intersecting interval *)
    ll := RefineAt( l, a ):
    ll := RefineAt( ll, b ):
    rr := RefineAt( r, a ):
    rr := RefineAt( rr, b ):

    (* Refiment can change type to rational. *)
    if rr:-isRational_ then
      return CompareRational( ll, rr:-a );
    elif ll:-isRational_ then
      return -CompareRational( rr, ll:-a );
    end if;

    (* Check if there is no intersection after refiment. *)
    if evalb( ll:-b < rr:-a ) then
      return -1;
    elif evalb( ll:-a > rr:-b ) then
      return 1;
    end if;

    (* The number of roots of the GCD of two polynomials is equal to the number of common roots.
       use this to simplify the problem in the intersecting range.*)
    G := gcd( ll:-poly, rr:-poly );
    F1 := simplify( ll:-poly / G );
    F2 := simplify( rr:-poly / G );

    if evalb( signum( eval( G, op( indets( G ) ) = ll:-a ) ) <> signum( eval( G,
      op( indets( G ) ) = ll:-b ) ) ) then
      ll := Object( ll, G, ll:-a, ll:-b ):
    else
      ll := Object( ll, F1, ll:-a, ll:-b ):
    end if:

    if evalb( signum( eval( G, op( indets( G ) ) = rr:-a ) ) <> signum( eval( G,
      op( indets( G ) ) = rr:-b ) ) ) then
      rr := Object( rr, G, rr:-a, rr:-b ):
    else
      rr := Object( rr, F2, rr:-a, rr:-b ):
    end if:

    (* Use of GCD can change type to rational. *)
    if rr:-isRational_ then
      return CompareRational( ll, rr:-a );
    elif ll:-isRational_ then
      return -CompareRational( rr, ll:-a );
    end if;

    (* Check for equality. *)
    if evalb( signum( eval( G, op( indets( G ) ) = a ) ) <> signum( eval( G,
      op( indets( G ) ) = b ) ) ) then
      return 0;
    end if;

    (* Refiment until disjoitness. *)
    for i from 1 while 1 = 1 do
      ll := BisectRange( ll ):
      rr := BisectRange( rr ):

      (* Rationals after refiment. *)
      if rr:-isRational_ then
        return CompareRational( ll, rr:-a );
      elif ll:-isRational_ then
        return -CompareRational( rr, ll:-a );
      end if;

      (* No intersection after refiment. *)
      if evalb( ll:-b < rr:-a ) then
        return -1;
      elif evalb( ll:-a > rr:-b ) then
        return 1;
      end if:
    end do:
  end proc:

# Method: < operator
#   A method used to compare two real algebraic numbers.
#
# Parameters:
#   l::RealAlgebraicNumber      - a real algebraic number
#   r::RealAlgebraicNumber      - a real algebraic number
#
# Output:
#   true when l is smaller than r and false otherwise.
#
  export `<`::static := proc( l, r, $ )
   if ( _npassed <> 2 or not l::RealAlgebraicNumber or not r::RealAlgebraicNumber ) then
    return false;
  end if;
  if Compare( l, r ) = -1 then
    return true;
  else
    return false;
  end if;
  end proc:

# Method: <= operator
#   A method used to compare two real algebraic numbers.
#
# Parameters:
#   l::RealAlgebraicNumber      - a real algebraic number
#   r::RealAlgebraicNumber      - a real algebraic number
#
# Output:
#   true when l is smaller or equal to r and false otherwise.
#
  export `<=`::static := proc( l, r, $ )
   if ( _npassed <> 2 or not l::RealAlgebraicNumber or not r::RealAlgebraicNumber ) then
    return false;
  end if;
  if Compare( l, r ) <= 0 then
    return true;
  else
    return false;
  end if;
  end proc:

# Method: = operator
#   A method used to compare two real algebraic numbers.
#
# Parameters:
#   l::RealAlgebraicNumber      - a real algebraic number
#   r::RealAlgebraicNumber      - a real algebraic number
#
# Output:
#   true when l is equal to r and false otherwise.
#
  export `=`::static := proc( l, r, $ )
    if Compare( l, r ) = 0 then
      return true;
    else
      return false;
    end if;
  end proc:

end module:


#
# Erstelle Liste aller Kinematikparameter
# Init
# Dieses Arbeitsblatt erstellt einen Vektor mit allen Kinematikparameter
# Es werden nur die Kinematikparameter eingetragen, die ungleich Null sind, also z.B. keine MDH-Parameter die Länge oder Winkel Null sind.
#
# TODO: Liste mit Kinematikparametern muss immer die selbe Reihenfolge haben. Am besten in der Form a,alpha,qoffset,d,b,beta,kc. Momentan scheint die Reihenfolge zufällig, aber reproduzierbar.
#
# Moritz Schappler, schappler@irt.uni-hannover.de, 2017-02
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
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): # Damit der ganze Parametervektor ausgegeben werden kann
;
with(LinearAlgebra):
read "../helper/proc_MatlabExport":
read "../helper/proc_convert_t_s":
codegen_act := true:
# Lese Umgebungsvariable für Codegenerierung.
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_twist_definitions", robot_name):
robot_name_OL := robot_name: # Zusätzliche Variable zur Abgrenzung für implizite Zwangsbedingungen
;
#  Schalte um zwischen impliziten ZB und normalen Systemen
if FileTools[Exists]("../workdir/tbmode") then
  read "../workdir/tbmode": # Datei tbmode wird von Bash-Skripten erstellt und zeigt den aktuellen Modus an
else
  tbmode := "serial":
end if:
if tbmode = "implicit" then
  printf("Modus für IC ist aktiv. Bestimme Kinematikparameter für System mit impliziten ZB.\n"):
  read "../robot_codegen_definitions/robot_env_IC":
end if:
printf("Generiere Kinematik-Parametervektor für %s\n",robot_name):
# Parameter der Zwangsbedingungen lesen
kin_constraints_exist := false:
kc_symbols2 := []:
# Für explizite Zwangsbedingungen
constrfile := sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_maple", robot_name_OL):
if FileTools[Exists](constrfile) then
  read constrfile:
  printf("Symbole der expliziten Zwangsbedingungen aus %s gelesen.\n", constrfile):
  kc_symbols2 := kc_symbols:
  kin_constraints_exist := true:
end if:
# Parameter für implizite Zwangsbedingungen (dadurch können auch neue Konstanten hinzugefügt werden)
constrfile := sprintf("../codeexport/%s/tmp/kinematic_implicit_constraints_symbols_list_maple", robot_name):
# Für implizite Zwangsbedingungen ist "robot_name" das System mit IC und nicht mit OL o.ä.
if FileTools[Exists](constrfile) then
  read constrfile:
  printf("Symbole der impliziten Zwangsbedingungen aus %s gelesen.\n", constrfile):
  kc_symbols2 := <kc_symbols2| kc_symbols>:
  kin_constraints_exist := true:
end if:
# Liste der Kinematikparameter erstellen
# Alle einzelnen MDH-Parametervektoren stapeln
pkin_tmp1 := <a;alpha;d;theta; qoffset;b;beta>:
# Kinematikparameter für kinematische Zwangsbedingungen hinzufügen (falls vorhanden)
if kin_constraints_exist then
  pkin_tmp1 := <pkin_tmp1;Transpose(kc_symbols2)>:
  np_kc := ColumnDimension(kc_symbols2):
else
  np_kc := 0:# Anzahl der Kinematikparameter für die Zwangsbedingungen
end if:
# Alle Symbole herausfinden
# Durch den Befehl fallen die vektoriell angesprochenen Gelenkkoordinaten (qJ) weg.
# TODO: Liste mit besserer Reihenfolge (siehe Dateikopf)
nms:=convert(indets(pkin_tmp1,name),list):
# Laufende Nummer für Parameterliste
kk := 0:
# Matrix mit Platz für alle Kinematikparameter (falls alle ungleich Null)
pkin_tmp2 := Matrix(NJ*6+np_kc,1):
# Terme aus Liste entfernen
for i from 1 to ColumnDimension(nms) do
  if nms[i] = Pi then # (wird im Code sowieso automatisch eingesetzt)
    next:
  end if:
  if nms[i] = t then # sollte eigentlich gar nicht drin sein können
    next:
  end if:
  # Parameter zur Liste hinzufügen
  kk := kk + 1:
  pkin_tmp2[kk,1] := nms[i]:
end do:
# Konstante Winkel, die aus MDH-Definition und nicht aus den ZB kommen umwandeln.
q_s := Matrix(1,1,0): q_t :=q_s:
pkin_tmp1 := convert_t_s(pkin_tmp1):
# Konvertierung nochmals durchführen (damit nicht delta8 und delta8s als verschiedene Symbole auftauchen)
pkin_tmp2 :=convert_t_s(pkin_tmp2):
# Ausgabevariable belegen
pkin := Matrix(pkin_tmp2[1..kk,1],kk,1):
# Symbole nochmals gruppieren
pkin := Transpose(Matrix(convert(indets(pkin,name),list))):
# Ergebnis speichern
# Für Generierung des Kinematikparametervektors in Matlab
MatlabExport(pkin, sprintf("../codeexport/%s/tmp/parameter_kin_matlab.m", robot_name), 2):
# Für schnelle Erkennung der Dimension zum Auslesen durch Bash-Skripte
save pkin, sprintf("../codeexport/%s/tmp/parameter_kin", robot_name):
# Zuordnung zwischen Kinematikparametern und MDH-Parametern
# Mit der Zuordnung kann der Vektor der Kinematikparameter (der an jede Funktion übergeben wird) aus den MDH-Parametern bestimmt werden
# TODO: Dieser Ansatz funktioniert nur, wenn keine Additionen von Variablen in den MDH-Parametern enthalten sind.
# Schritt 1: pkin_tmp1 nachverarbeiten: Gelenkvariablen und Pi entfernen (damit der solve-Befehl weniger zu verarbeiten hat)
pkin_tmp3 := pkin_tmp1: # Variable, die gestapelt eine nachbearbeitete Version aller MDH-Parameter enthält
;
for i from 1 to RowDimension(pkin_tmp3) do
  # q entfernen
  for j from 1 to RowDimension(qJ_t) do
    pkin_tmp3(i,1) := subs({qJ_t(j,1)=0}, pkin_tmp3(i,1)):
  end do:
  # Term Null setzen, wenn keine Variablen drin stehen (z.B. nur Pi)
  nms:=convert(indets(pkin_tmp3(i),name),list):
  var_da := 0: # Zähler, ob Variablen enthalten sind
  for k from 1 to ColumnDimension(nms) do
    if nms[k] = Pi then # Pi ist Konstante und funktioniert später nicht im Solve-Befehl
      next:
    end if:
    var_da := 1:
  end do:
  if var_da = 0 then
    pkin_tmp3(i,1) := 0:
  end if:
end do:

pkin_subs_mdh:=copy(pkin): # ohne "copy" wird anscheinend verlinkt und der Rest geht nicht
;
pkt3i := Matrix(RowDimension(pkin_tmp3), 1): # Index-Vektor für pkin_tmp3
;
for i from 1 to RowDimension(pkin_tmp3) do
  pkt3i(i,1):=parse(sprintf("pp%03d", i)): # Hilfsgröße zum späteren Ersetzen
end do:
# Ersetze

for i from 1 to RowDimension(pkin_tmp3) do
  if pkin_tmp3(i) = 0 then
    next:
  end if:
  for j from 1 to RowDimension(pkin) do
    # printf("i=%d, j=%d\n", i, j):
    erg := solve({pkin_tmp3(i)=pkt3i(i)}, pkin(j)):
    if ArrayTools:-NumElems(erg) = 0 then
      next:
    end if:
    # printf("i=%d\n", i):
    pkin_subs_mdh(j) := subs({lhs(erg[1])=rhs(erg[1])}, pkin_subs_mdh(j)):
    break: # Der erste Fund reicht
  end do:
end do:

# Ergebnis nachverarbeiten
# Platzhalter ("ph") generieren (für die Eingabeargumente der Matlab-Funktion zur Berechnung der Parameter)
beta_ph := Matrix(NJ,1):
b_ph := Matrix(NJ,1):
alpha_ph := Matrix(NJ,1):
a_ph := Matrix(NJ,1):
theta_ph := Matrix(NJ,1):
d_ph := Matrix(NJ,1):
qoffset_ph := Matrix(NJ,1):
for i from 1 to NJ do
  beta_ph(i) := parse(sprintf("beta_mdh(%d)", i)):
  b_ph(i) := parse(sprintf("b_mdh(%d)", i)):
  alpha_ph(i) := parse(sprintf("alpha_mdh(%d)", i)):
  a_ph(i) := parse(sprintf("a_mdh(%d)", i)):
  theta_ph(i) := parse(sprintf("theta_mdh(%d)", i)):
  d_ph(i) := parse(sprintf("d_mdh(%d)", i)):
  qoffset_ph(i) := parse(sprintf("qoffset_mdh(%d)", i)):
end do:
# Platzhalter-Vektor zur Ersetzung der gestapelten MDH-Parameter
pkt1_ph := <a_ph;alpha_ph;d_ph;theta_ph; qoffset_ph;b_ph;beta_ph>:
# Kinematikparameter für kinematische Zwangsbedingungen hinzufügen (falls vorhanden)
#TODO (aktuell wird das Problem mit der NaN-Ersetzung unten umgangen).
;
# Parameter-Indizes mit den Platzhaltern für die Matlab-Funktion ersetzen
for i from 1 to RowDimension(pkin_subs_mdh) do
  for j from 1 to RowDimension(pkt1_ph) do
    pkin_subs_mdh(i) := subs( {pkt3i(j)=pkt1_ph(j)}, pkin_subs_mdh(i) ):
  end do:
end do:
# Ersetze alle noch nicht ersetzten Kinematikparameter mit NaN. Ansonsten ist der Matlab-Code nicht lauffähig und es treten nachgelagerte Probleme auf.
for i from 1 to RowDimension(pkin_subs_mdh) do
  for j from 1 to RowDimension(pkt3i) do
    if has(pkin_subs_mdh(i), pkt3i(j)) then
      printf("Die Variable %s kommt noch in der Umrechnung mdh->pkin vor. Vermutlich Fehler beim substituieren. Setze NaN.\n", String(pkt3i(j))):
      pkin_subs_mdh(i) := subs( {pkt3i(j)=NaN}, pkin_subs_mdh(i) ):
    end if:
  end do:
  for j from 1 to RowDimension(kintmp_s) do
    if has(pkin_subs_mdh(i), kintmp_s(j)) then
      printf("Die Variable %s kommt noch in der Umrechnung mdh->pkin vor. Vermutlich Fehler beim substituieren. Setze NaN.\n", String(kintmp_s(j))):
      pkin_subs_mdh(i) := subs( {kintmp_s(j)=NaN}, pkin_subs_mdh(i) ):
    end if:

  end do:
end do:

# Folgender Term muss immer erzeugt werden (wird für Bash-Skripte benötigt. Sonst dort Fehler und Abbruch).
interface(warnlevel=0): # Unterdrücke die folgende Warnung (weil MDH-Parameter als Funktionsnamen interpretiert werden. Code funktioniert trotzdem.)
MatlabExport(pkin_subs_mdh, sprintf("../codeexport/%s/tmp/parameter_kin_from_mdh_matlab.m", robot_name), 2):
interface(warnlevel=3):

# Ausgabe
# MDH-Tabelle ausgeben
interface(rtablesize=100):
Test := <<seq(i, i=1..NJ)> | sigma| mu| beta| b | alpha | a | theta | d | v>:
Test:=<<"i" | "sigma" | "mu"|"beta"|"b"|"alpha"|"a"|"theta"|"d"|"v">,Test>:
printf("MDH-Tabelle für %s:\n", robot_name):
Test;
printf("Kinematik-Parameter für %s: %dx%d\n", robot_name, RowDimension(pkin), ColumnDimension(pkin)):
Transpose(pkin);