`is_element/digraphs` := (V::set) -> proc(E)
 if not(`is_element/autorel`(V)(E)) then
  return false;
 fi;

 if not(`is_antisymmetric/autorel`(V)(E)) then
  return false;
 fi;

 return true;
end:

`is_leq/digraphs` := (V::set) -> (E0,E1) -> evalb(E0 minus E1 = {}):

`list_elements/digraphs` := proc(V::set)
 local n,EE,E,i,j,a,b;

 n := nops(V);
 EE := [{}];
 for i from 1 to n-1 do
  for j from i+1 to n do
   a := V[i];
   b := V[j];
   EE := [op(EE),
          seq(E union {[a,b]},E in EE),
          seq(E union {[b,a]},E in EE)];
  od;
 od;

 return EE;
end:

`count_elements/digraphs` := (V::set) -> 3^(nops(V)*(nops(V)-1)/2);

`random_element/digraphs` := (V::set) -> proc()
 local n,E,r,i,j,a,b,x;

 n := nops(V);
 E := NULL;
 r := rand(-1..1);
 for i from 1 to n-1 do
  for j from i+1 to n do
   a := V[i];
   b := V[j];
   x := r();
   if x = -1 then
    E := E,[b,a];
   elif x = 1 then
    E := E,[a,b];
   fi;
  od;
 od;

 return {E};
end:

`neighbour_table/digraphs` := (V) -> proc(E)
 local N,v,e;

 N := table();
 for v in V do N[v] := {}; od:
 for e in E do
  N[e[1]] := {op(N[e[1]]),e[2]};
  N[e[2]] := {op(N[e[2]]),e[1]};
 od:

 return eval(N);
end:

`in_neighbour_table/digraphs` := (V) -> proc(E)
 local N,v,e;

 N := table();
 for v in V do N[v] := {}; od:
 for e in E do
  N[e[2]] := {op(N[e[2]]),e[1]};
 od:

 return eval(N);
end:

`out_neighbour_table/digraphs` := (V) -> proc(E)
 local N,v,e;

 N := table();
 for v in V do N[v] := {}; od:
 for e in E do
  N[e[1]] := {op(N[e[1]]),e[2]};
 od:

 return eval(N);
end:

`component_relation/digraphs` := (V) -> (E) ->
 `transitive_closure/autorel`(V)(
  E union `op/autorel`(V)(E) union `id/autorel`(V)
 );

`components/digraphs` := (V) -> (E) ->
 `block_partition/equiv`(`component_relation/graphs`(V)(E));

`is_connected/digraphs` := (V) -> (E) ->
 evalb(`component_relation/digraphs`(V)(E) = `top/autorel`(V));

`skeleton/digraphs` := (V::set) -> proc(E)
 local n,V0,E0,ix,i;

 n := nops(V);
 V0 := {seq(i,i=1..n)};
 ix := table():
 for i from 1 to n do ix[V[i]] := i; od;
 E0 := map(e -> [ix[e[1]],ix[e[2]]],E);
 return [V0,E0];
end:

`is_element/dipaths` := (V) -> (E) -> proc(p)
 local i,n;

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

 n := nops(p) - 1;

 if n < 0 then
  return false;
 fi;

 if {op(p)} minus V <> {} then
  return false;
 fi;

 for i from 1 to n do
  if not(member([p[i],p[i+1]],E)) then
   return false;
  fi;
 od;

 return true;
end:

`is_small_element/dipaths` := (V) -> (E) -> (n::nonnegint) -> proc(p)
 return evalb(`is_element/dipaths`(V)(E)(p) and (nops(p) = n+1));
end:

`random_small_element/dipaths` := (V) -> (E) -> (n::nonnegint) -> proc(num_tries := 10)
 local k,p,F;

 if n = 0 then
  if V = {} then
   return FAIL;
  else
   return [random_element_of(V)()];
  fi;
 elif n = 1 then
  if E = {} then
   return FAIL;
  else
   return random_element_of(E)();
  fi;
 fi;

 k := num_tries;

 while k > 0 do
  k := k-1;
  p := `random_small_element/dipaths`(V)(E)(n-1)();
  if p <> FAIL then
   F := select(e -> e[2] = p[1],E);
   if F <> {} then
    return [random_element_of(F)()[1],op(p)];
   fi;
  fi;
 od;

 return FAIL;
end:

`list_small_elements/dipaths` := (V) -> (E) -> proc(n::nonnegint)
 option remember;
 local P,Q,p,v,F,e;

 if n = 0 then
  return map(v -> [v],V);
 else
  P := `list_small_elements/dipaths`(V)(E)(n-1);
  Q := NULL;
  for p in P do
   v := p[1];
   F := select(e -> e[2] = v,E);
   Q := Q,seq([e[1],op(p)],e in F);
  od:
  return [Q];
 fi;
end:

`is_leq/dipaths` := NULL;
`count_elements/dipaths` := NULL:
`count_small_elements/dipaths` := NULL:

`length/dipaths` := (V) -> (E) -> (p) -> nops(p) - 1;

`is_trail/dipaths` := proc(p)
 local n,e,i;
 n := nops(p) - 1;
 e := {seq([p[i],p[i+1]],i=1..n)};
 return evalb(nops(e) = n);
end:

`is_cycle/dipaths` := proc(p)
 local n,i;
 n := nops(p) - 1;
 return evalb(p[1] = p[n+1] and nops({seq(p[i],i=1..n)}) = n);
end:

`is_small_element/ditrails` := (V) -> (E) -> (n::nonnegint) -> proc(p)
 if not `is_small_element/dipaths`(V)(E)(n)(p) then
  return false;
 fi;

 return `is_trail/dipaths`(p);
end:

`is_element/ditrails` := (V) -> (E) -> proc(p)
 if not `is_element/dipaths`(V)(E)(p) then
  return false;
 fi;

 return `is_trail/dipaths`(p);
end:

`list_small_elements/ditrails` := (V) -> (E) -> proc(n)
 option remember;
 local E0,E1,P,Q,p,v,F,i,e;

 if n = 0 then
  return map(v -> [v],V);
 else
  P := `list_small_elements/ditrails`(V)(E)(n-1);
  Q := NULL;
  for p in P do
   E0 := {seq([p[i],p[i+1]],i=1..n-1)};
   E1 := E minus E0;
   v := p[1];
   F := select(e -> e[2] = v,E1);
   Q := Q,seq([e[1],op(p)],e in F);
  od:
  return [Q];
 fi;
end:

`random_small_element/ditrails` := (V) -> (E) -> (n) -> proc(num_tries := 10)
 local i,k,p,E0,E1,v,F,e;

 k := num_tries;

 if n = 0 then
  if V = {} then
   return FAIL;
  else
   return [random_element_of(V)()];
  fi;
 elif n = 1 then
  if E = {} then
   return FAIL;
  else
   return random_element_of(E)();
  fi;
 fi;

 while k > 0 do
  p := `random_small_element/ditrails`(V)(E)(n-1)();
  k := k - 1;
  if p <> FAIL then
   E0 := {seq([p[i],p[i+1]],i=1..n-1)};
   E1 := E minus E0;
   v := p[1];
   F := select(e -> e[2] = v,E1);
   if F <> {} then
    e := random_element_of(F)();
    return [e[1],op(p)];
   fi;
  fi;
 od;

 return FAIL;
end:

`is_leq/ditrails` := NULL:
`count_elements/ditrails` := NULL:
`count_small_elements/ditrails` := NULL:

`roots/digraphs` := (V) -> proc(E)
 V minus map(e -> e[2],E);
end:

`root/digraphs` := (V) -> proc(E)
 local R;
 R := `roots/digraphs`(V)(E);
 if nops(R) = 1 then
  return R[1];
 else
  error "Digraph does not have a unique root";
 fi;
end:

`leaves/digraphs` := (V) -> proc(E)
 V minus map(e -> e[1],E);
end:

`is_forest/digraphs` := (V::set) -> proc(E)
 local NI,NO,R,v,U,U1;

 NI := `in_neighbour_table/digraphs`(V)(E);
 NO := `out_neighbour_table/digraphs`(V)(E);
 R := NULL;

 for v in V do
  if nops(NI[v]) > 1 then
   return false;
  elif NI[v] = {} then
   R := R,v;
  fi;
 od;

 R := {R};

 U := R;
 U1 := {op(U),seq(op(NO[v]),v in U)};
 while nops(U1) > nops(U) do
  U := U1;
  U1 := {op(U),seq(op(NO[v]),v in U)};
 od:

 if U <> V then return false; fi;

 return true;
end:

`is_tree/digraphs` := (V) -> (E) ->
 nops(`roots/digraphs`(V)(E)) = 1 and
  `is_forest/digraphs`(V)(E);

`level_table/forest_digraphs` := (V) -> proc(E)
 local X,N,L,k,v;

 X := `roots/digraphs`(V)(E);
 N := `out_neighbour_table/digraphs`(V)(E);
 L := table():
 k := 0;
 while X <> {} do
  for v in X do L[v] := k; od:
  X := map(v -> op(N[v]),X);
  k := k+1;
 od:

 return eval(L);
end:

`shadow_table/forest_digraphs` := (V) -> proc(E)
 local X,Y,Z,S,x,z,N,u;

 X := `leaves/digraphs`(V)(E);
 Y := V minus X;
 S := table():
 for x in X do S[x] := {x}; od:
 N := `out_neighbour_table/digraphs`(V)(E);
 while Y <> {} do
  Z := select(y -> ((N[y] minus X) = {}),Y);
  if Z = {} then
   error "Something wrong";
  fi;
  z := Z[1];
  S[z] := map(op,{seq(S[u],u in N[z])});
  X := X union {z};
  Y := Y minus {z};
 od:
 return eval(S);
end:

`shadow_set/forest_digraphs` := (V) -> proc(E)
 local S,v;
 S := `shadow_table/forest_digraphs`(V)(E);
 return {seq(S[v],v in V)};
end:

`is_element/stasheff_digraphs` := (V::set) -> proc(EL)
 local E,L,L0,r,i,TT,rTT;
 if not (type(EL,list) and nops(EL) = 2) then
  return false;
 fi;

 E,L := op(EL);

 if not(`is_tree/digraphs`(V)(E)) then
  return false;
 fi;

 L0 := `leaves/digraphs`(V)(E);
 if not({op(L)} = L0 and nops(L0) = nops(L)) then
  return false;
 fi;

 r := table();
 for i from 1 to nops(L) do r[L[i]] := i; od;

 TT := `shadow_set/forest_digraphs`(V)(E);
 rTT := map(T -> map(v -> r[v],T),TT);

 return `and`(op(map(`is_element/posint_intervals`,rTT)));
end:


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: work_mgga_x *)

$include "mgga_x_gx.mpl"

mmu := 0.001015549:
pbe_gx := x -> 1/(1 + mmu*x^2):

f := (rs, x, t, u) ->
  f_gx_a(malpha(x, t)) * pbe_gx(x):


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: work_gga_c *)
(* prefix:
  gga_c_sogga11_params *params;

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

$define lda_c_pw_params
$define lda_c_pw_modified_params
$include "lda_c_pw.mpl"

mbeta  := 15.75592*0.004235: (* the usual value of 0.066726 *)
malpha := mbeta/(16*2^(2/3)):

yy := (rs, z, xt) -> -malpha*mphi(z)*xt^2/(rs*f_pw(rs, z)):

f0 := (rs, z, xt) -> 1 - 1/(1 + yy(rs, z, xt)):
f1 := (rs, z, xt) -> 1 - exp(-yy(rs, z, xt)):

t0 := (rs, z, xt) -> add(params_a_sogga11_a[i]*f0(rs, z, xt)^(i-1), i=1..6):
t1 := (rs, z, xt) -> add(params_a_sogga11_b[i]*f1(rs, z, xt)^(i-1), i=1..6):

f_sogga11 := (rs, z, xt, xs0, xs1) ->
  f_pw(rs, z)*(t0(rs, z, xt) + t1(rs, z, xt)):

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


# Proof by PDF that X <~ standardChiSq(2) == Y <~ exponential(2)

PDF_chisq := (n, x) -> x^(1/2*n-1)*exp(-1/2*x)/(2^(1/2*n))/GAMMA(1/2*n);
simplify(PDF_chisq(2,x));

PDF_exponential := (alpha, x) -> exp(-x/alpha)/alpha;
simplify(PDF_chisq(2,x));

evalb(PDF_chisq(2,x) = PDF_exponential(2,x));


(*
 Copyright (C) 2017 M.A.L. Marques
               2019 Susi Lehtola

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_exc *)
(* prefix:
  mgga_x_gx_params *params;

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

$ifdef mgga_x_gx_params
params_a_c0       :=  0.827411:
params_a_c1       := -0.643560:
params_a_alphainf :=  0.852:
$endif

gx_alpha := (x, t) -> (t - x^2/8)/K_FACTOR_C:

gx_cx0 := 4/3*(2/Pi)^(1/3):
gx_cx1 := X_FACTOR_C:

gx_gx0 := a ->
  + gx_cx0/gx_cx1
  + a*(params_a_c0 + params_a_c1*a)/(1.0 + (params_a_c0 + params_a_c1 - 1)*a) * (1 - gx_cx0/gx_cx1):

gx_gx1 := a ->
  1 + (1 - params_a_alphainf)*(1 - a)/(1 + a):

gx_f_a := a->
  + gx_gx0(a)*Heaviside(1 - a)
  + gx_gx1(a)*Heaviside(a - 1):

gx_f := (x, u, t) ->
  gx_f_a(gx_alpha(x, t)):

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


$ifndef _CLOSURE_
$define _CLOSURE_

# 寻找变换矩阵的闭包
# 代入的条件只能是等于0的条件，目的在于删去一些系数之后再求闭包
getClosure:=proc(A::Matrix,sol:={})
    local _a,_b,n;
    n:=LinearAlgebra[RowDimension](A);
    _a:=Matrix([seq(a[i],i=1..n)]);
    _a:=subs(sol[],_a);
    _b:=_a.A;
    _b:=map(x->sym2ind(indets(x,specindex(a))),convert(_b,list));
    return map(findClosure,[seq(1..n)],_b);
end proc:

# 提取a[k]的下标
sym2ind:=proc(s::set)
    return map(x->op(1,x),s);
end proc:

# 寻找某个元素所在的最小闭包
findClosure:=proc(k::integer,A::list)
    local aset,bset;
    aset:={};
    bset:=A[k];
    while aset<>bset do
        aset:=bset;
        bset:=`union`(bset,A[convert(bset,list)][]);
    end do;
    return aset;
end proc:

# 获取最小封闭
# 输入参数为封闭的输出结果
# 忽略空集
getMinClosure:=proc(c::list(set))
    local ns,ind;
    ns:=map(x->`if`(x={},infinity,numelems(x)),c);
    ind:=min[index](ns);
    return c[ind];
end proc:

$endif


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_exc *)
(* prefix:
  mgga_x_br89_params *params;

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

$include "mgga_x_br89.mpl"

(* This is only used in B00 *)
params_a_at := 0:

br89_y := Q -> 2*Pi^(2/3)/(3*Q):

(* lower piece *)
pgk_a1 := 1.5255251812009530:
pgk_a2 := 0.4576575543602858:
pgk_a3 := 0.4292036732051034:

pgk_b  := [0.4771976183772063, -1.7799813494556270, 3.8433841862302150,
       -9.5912050880518490, 2.1730180285916720, -30.425133851603660]:

pgk_c  := [0.7566445420735584, -2.6363977871370960, 5.4745159964232880,
       -12.657308127108290, 4.1250584725121360, -30.425133957163840]:

pgk_d  := [0.00004435009886795587, 0.58128653604457910, 66.742764515940610,
       434.26780897229770, 824.7765766052239000, 1657.9652731582120]:

pgk_e  := [0.00003347285060926091, 0.47917931023971350, 62.392268338574240,
       463.14816427938120, 785.2360350104029000, 1657.962968223273000000]:

pgk_UB := 2.085749716493756:

pgk_x_lower := y -> (-arctan(pgk_a1*y + pgk_a2) + pgk_a3) *
            add(pgk_c[i]*y^(i-1), i=1..6)/add(pgk_b[i]*y^(i-1), i=1..6):

pgk_x_upper := y -> (arccsch(pgk_UB*y) + 2) *
            add(pgk_d[i]*y^(i-1), i=1..6)/add(pgk_e[i]*y^(i-1), i=1..6):

br89_mx := Q -> my_piecewise3(
  br89_y(Q) <= 0,
  pgk_x_lower(m_min(br89_y(Q), -br89_min_Q)),
  pgk_x_upper(m_max(br89_y(Q),  br89_min_Q))
):


# Center of Mass Jacobian for the Robot based on MDH frames
# Introduction
# Berechnung der Schwerpunkts-Jacobi-Matrix (Veränderung des Schwerpunktes in Abhängigkeit der Gelenkwinkel).
# Diese Matrix wird bei Gleichgewichtsregelungen (z.B. zweibeiniges Laufen) gebraucht.
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# tree -> Berechnung für eine beliebige Baumstruktur (ohne Schleifen)
# floatb -> floating base wird durch base twist (Geschwindigkeit der Basis) oder vollständige Orientierung (Euler-Winkel) berücksichtigt
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# kinematics_comjacobi -> Jacobi-Matrix der Körperschwerpunkte
# worldframe -> Berechnung der Geschwindigkeit im Welt-KS (KSW)
# par1 -> Parametersatz 1 (Schwerpunkt als Parameter: SX,SY,SZ)
# Authors
# Moritz Schappler, schappler@irt.uni-hannover.de, 2015-12
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
#
# Sources
# [SugiharaNakIno2002] Sugihara, T. and Nakamura, Y. and Inoue, H.: Real-time humanoid motion generation through ZMP manipulation based on inverted pendulum control (2002)
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(ArrayTools):
with(codegen):
with(CodeGeneration):
with(StringTools):
codegen_act := true:
codegen_opt := 2:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotx":
read "../transformation/proc_troty":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../transformation/proc_trafo_mdh":
read "../robot_codegen_definitions/robot_env":
printf("Generiere Schwerpunktskinematik für %s\n", robot_name):
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
# Ergebnisse der Kinematik laden
read sprintf("../codeexport/%s/tmp/kinematics_floatb_%s_rotmat_maple.m", robot_name, base_method_name):
Trf := Trf:
Trf_c := Trf_c:
# Schwerpunkt des Roboters laden
read sprintf("../codeexport/%s/tmp/kinematics_com_total_worldframe_floatb_%s_par1_maple.m", robot_name, base_method_name):
# Calculate CoG Jacobian
# CoG Jacobian nach [SugiharaNakIno2002].
# TODO: baseframe in worldframe ändern
J_COG_s := Matrix(3, NQJ):
c_s := convert_t_s(c):
for i from 1 to 3 do
  for j from 1 to NQJ do
    J_COG_s(i,j) := diff(c_s(i,1), qJ_s(j,1)):
  end:
end:
# TODO: Wenn der Ausdruck J_COG_s nicht gespeichert und wieder geladen wird, hängt die Berechnung bei der Optimierung des Ausdrucks mit "tryhard"
save J_COG_s, sprintf("../codeexport/%s/tmp/kinematics_Jcom_worldframe_floatb_%s_par1_maple.m", robot_name, base_method_name):
read sprintf("../codeexport/%s/tmp/kinematics_Jcom_worldframe_floatb_%s_par1_maple.m", robot_name, base_method_name):
if codegen_act then
  MatlabExport(J_COG_s, sprintf("../codeexport/%s/tmp/com_jacobi_baseframe_par1_matlab.m", robot_name), 2):
end if:
# CoG-Jacobian Time derivative
J_COG := convert_s_t(J_COG_s):
JD_COG := diff~(J_COG, t):
JD_COG_s := convert_t_s(JD_COG):
if codegen_act then
  MatlabExport(convert_t_s(JD_COG_s), sprintf("../codeexport/%s/tmp/com_jacobiD_baseframe_par1_matlab.m", robot_name), codegen_opt):
end if:


var $arr1 <[10] f32>

func $ArrayAdd () f32 {
 var %ff i32
 var %sum f32
 dassign %sum ( constval f32 0.0f )
 foreachelem %ff $arr1 {
   dassign %sum ( add f32 ( dread f32 %sum, dread f32 %ff ) )
 }
 return ( dread f32 %sum )
}
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl


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

`is_element/preord` := (A::set) -> proc(R)
 global reason;

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

 if not(`is_reflexive/autorel`(A)(R)) then
  reason := [convert(procname,string),"R is not reflexive",R];
  return false;
 fi;

 if not(`is_transitive/autorel`(A)(R)) then
  reason := [convert(procname,string),"R is not transitive",R];
  return false;
 fi;

 return true;
end;

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

`is_equal/preord`     := eval(`is_equal/autorel`);
`is_separated/preord` := eval(`is_separated/autorel`);

# We order the set of preorders by reverse inclusion, so that
# separated preorders are maximal.

`is_leq/preord`       := (A::set) -> proc(R,S)
 evalb(S minus R = {});
end;

`op/preord` := eval(`op/autorel`);

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

`random_element/preord` := (A::set) -> proc(R)
 local pi,n,r,i,a,b,N,RN,A2,RA;

 pi := `random_element/partitions`(A)();
 n := nops(pi);
 r := table();
 for i from 1 to n do
  for a in pi[i] do
   r[a] := i;
  od:
 od:
 N := {seq(i,i=1..n)};
 RN := `random_element/partord`(N)();
 A2 := {seq(seq([a,b],b in A),a in A)};
 RA := select(u -> member([r[u[1]],r[u[2]]],RN),A2);

 return RA;
end:

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

`is_total/preord` := (A::set) -> proc(R)
 local U,a,b;

 U := R union `op/preord`(A)(R);
 U := {seq(seq([a,b],b in A),a in A)} minus U;

 return evalb(U = {});
end;

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

`strictify/preord` := (A::set) -> proc(R)
 return R minus `op/preord`(A)(R);
end:

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

`res/preord` := (A::set,B::set) -> (R) ->
 select(xy -> member(xy[1],B) and member(xy[2],B),R);

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

`block_partition/preord` := (A::set) -> proc(R)
 local E,pi,X,x,B;

 E := R intersect map(ab -> [ab[2],ab[1]],R);
 pi := NULL;
 X := A;

 while X <> {} do
  x := X[1];
  B := map(ab -> ab[2],select(ab -> ab[1] = x,E));
  pi := pi,B;
  X := X minus B;
 od:

 pi := {pi};

 return pi;
end:

######################################################################
# Note that the function below is not actually a rank function, but
# it is an ingredient in certain rank functions to be defined
# elsewhere.

`rank/preord` := (A::set) -> proc(R)
 nops(`block_partition/preord`(A)(R));
end:

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

`bump/preord` := (A::set) -> proc(R)
 local pi,B,a,b,B0,R0;

 pi := `block_partition/preord`(A)(R);

 for B in pi do
  if nops(B) > 1 then
   a := B[1];
   B0 := B minus {a};
   R0 := R minus {seq([b,a],b in B0)};
   return R0;
  fi;
 od;

 return FAIL;
end:

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

`rank_table/preord` := (A::set) -> proc(R)
 local pi,pi0,a,b,r,k,B;

 pi := `block_partition/preord`(A)(R);
 pi0 := map(B -> B[1],pi);
 r := table():
 for B in pi do
  k := nops(select(a -> member([a,B[1]],R),pi0)) - 1;
  for b in B do r[b] := k; od;
 od;

 return eval(r);
end;

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

`separated_quotient/preord` := (A::set) -> proc(R)
 local A0,R0,a0,b0;

 A0 := `block_partition/preord`(A)(R);
 R0 := NULL;
 for a0 in A0 do
  for b0 in A0 do
   if member([a0[1],b0[1]],R) then
    R0 := R0,[a0,b0];
   fi;
  od;
 od;
 R0 := {R0};

 return [A0,R0];
end:

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

`preord_is_leq` := (A::set) -> (R) -> (a,b) -> member([a,b],R);

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

`preord_is_equiv` := (A::set) -> (R) -> (a,b) ->
  member([a,b],R) and member([b,a],R);

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

`preord_is_less` := (A::set) -> (R) -> (a,b) ->
  member([a,b],R) and not(member([b,a],R));


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: work_gga_c *)

$include "gga_c_tca.mpl"

msinc := x -> piecewise(x = 0, 1, sin(x)/x):
aa    := Pi*(9*Pi/4)^(1/3):

fD := (rs, z, s) -> 1 - z^4*(1 - msinc(aa*s/rs)^2):

f := (rs, z, xt, xs0, xs1) ->
  f_tcs(rs, z, xt)*fD(rs, z, X2S*2^(1/3)*xt):


knights_tour_map := [
 [ 0, 5, 2,57,62,15,18,21],
 [ 3,56,63,14, 9,20,61,16],
 [ 6, 1, 4,43,58,17,22,19],
 [55,44,27, 8,13,10,39,60],
 [28, 7,42,45,40,59,12,23],
 [51,54,49,26,11,36,33,38],
 [48,29,52,41,46,31,24,35],
 [53,50,47,30,25,34,37,32]
];

knights_tour_pos := table():
knights_tour_opos := table():

for i from 0 to 7 do
 for j from 0 to 7 do
  knights_tour_pos[knights_tour_map[i+1,j+1]] := [i,j];
  knights_tour_opos[knights_tour_map[i+1,j+1]] := [i,j] +~ [0.5,0.5];
 od:
od:
knights_tour_pos[64] := knights_tour_pos[0]:
knights_tour_opos[64] := knights_tour_opos[0]:

chess_grid := display(
 seq(line([i,0],[i,8],colour=grey),i=0..8),
 seq(line([0,i],[8,i],colour=grey),i=0..8),
 scaling=constrained,axes=none
):

shaded_chess_grid := display(
 seq(seq(rectangle([2*i+1,2*j+1],[2*i+2,2*j],colour=grey),j=0..3),i=0..3),
 seq(seq(rectangle([2*i,2*j+2],[2*i+1,2*j+1],colour=grey),j=0..3),i=0..3),
 chess_grid
);

knights_tour_plot := display(
 shaded_chess_grid,
 seq(line(knights_tour_opos[i],knights_tour_opos[i+1],colour=red),i=0..63),
 scaling=constrained,axes=none
);

is_knight_move := (u,v) -> evalb({op(map(abs,u -~ v))} = {1,2});

check_knights_tour := proc()
 evalb(sort(map(op,knights_tour_map)) = [seq(i,i=0..63)]) and
 `and`(seq(is_knight_move(knights_tour_pos[i],knights_tour_pos[i+1]),i=0..63));
end:


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)

sg4_mu1     := 0.042:
sg4_mu2     := 0.26 - sg4_mu1:
sg4_nu_MGE4 := -0.195:
sg4_k2      := -sg4_mu2^2/sg4_nu_MGE4:
sg4_k1      := 0.804 - sg4_k2:

sg4_f0 := s -> 1 + sg4_k1 + sg4_k2
   - sg4_k1*(1 - sg4_mu1*s^2/sg4_k1)/(1 - (sg4_mu1*s^2/sg4_k1)^5)
   - sg4_k2/(1 + sg4_mu2*s^2/sg4_k2):
sg4_f:= x -> sg4_f0(X2S*x):

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


(*
 Copyright (C) 2017 M.A.L. Marques
               2019 Susi Lehtola

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)
(* prefix:
  gga_c_am05_params *params;

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

$define lda_c_pw_params
$define lda_c_pw_modified_params
$include "lda_c_pw.mpl"

XX := s -> 1/(1 + params_a_alpha*s^2):
ff := s -> XX(s) + params_a_gamma*(1 - XX(s)):

f := (rs, z, xt, xs0, xs1) -> f_pw(rs, z)*(
  + opz_pow_n( z,1)/2 * ff(X2S*xs0)
  + opz_pow_n(-z,1)/2 * ff(X2S*xs1)
):


# Berechne kinematische Zwangsbedingungen für Fünfgelenkkette

# Einleitung
# Die kinematischen Zwangsbedingungen werden als Ersetzungsausdruck für die abhängigen Winkel aufgestellt.
#
# fivebar1TE -> Fünfgelenkkette, Modellierung der Zwangsbedingungen mit trigonometrischer Elimination der Winkel
# kinematic_constraint -> Berechnungen zu kinematischen Zwangsbedingungen

# Quelle
# SA Bejaoui: Bejaoui2018_S749; "Modellierung kinematischer Zwangsbedingungen für hybride serielle Roboter mit planaren Parallelmechanismen"
# (Fünfgelenkkette S. 23 in [Bejaoui2018_S749])
# Autor
# Abderahman Bejaoui, Moritz Schappler
# Studienarbeit bei: Moritz Schappler, moritz.schappler@imes.uni-hannover.de, 2019-01
# (C) Institut fuer Mechatronische Systeme, Leibniz Universitaet Hannover
# Initialisierung
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
kin_constraints_exist := true: # Für Speicherung
;
with(StringTools): # Für Zeitausgabe
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
#with(ListTools):
codegen_act := true:
codegen_opt := 2:
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../robot_codegen_constraints/proc_subs_kintmp_exp":
read "../helper/proc_intersect_circle":
with(RealDomain): # Schränkt alle Funktionen auf den reellen Bereich ein. Muss nach Definition von MatlabExport kommen. Sonst geht dieses nicht.
;
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions",robot_name):
use_simplify := 0: # keine zusätzlichen simplify-Befehle
;
# Variable mit Winkeln der Nebenstruktur nur in Abhängigkeit der verallgemeinerten Koordinaten
kintmp_qs := Matrix(RowDimension(kintmp_s),1):
for i from 1 to RowDimension(kintmp_s) do
  if diff(kintmp_s(i,1), t) = 0 then
    kintmp_qs(i,1) := kintmp_s(i,1):
  end if:
end do:
# Ersetzungsausdrücke definieren.
# Speichere Sinus und Cosinus der Winkel direkt ab, da diese in den Rotationsmatrizen direkt auftreten.
# Spalte 1: Zu suchender Ausdruck (sin oder cos eines Winkels)
# Spalte 2: Einzusetzender Ausdruck.
# Dadurch werden arctan-Ausdrücke in der direkten Kinematik reduziert.
kintmp_subsexp := Matrix(2*RowDimension(kintmp_s),2):
for i from 1 to RowDimension(kintmp_s) do
  kintmp_subsexp(2*i-1, 1) := sin(kintmp_s(i,1)):
  kintmp_subsexp(2*i,   1) := cos(kintmp_s(i,1)):
  # Initialisierung der rechten Spalte mit gleichen Werten. Später nur Ersetzung, wenn Vorteilhaft.
  kintmp_subsexp(2*i-1, 2) := kintmp_subsexp(2*i-1, 1):
  kintmp_subsexp(2*i,   2) := kintmp_subsexp(2*i,   1):
end do:
printf("%s. Beginn der Berechnungen.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
st := time():
# Zwangsbedingungen mit Kreisschnittpunkt D zur Bestimmung von rho
r_p1:=<AE*cos(qJ1s), AE*sin(qJ1s)>:
r_p2:=<AB+BC*cos(qJ2s), BC*sin(qJ2s)>:
r1:=ED:
r2:=CD:
A:=intersect_circle(r_p1, r_p2, r1, r2):
# Es gibt zwei Lösungen für den Kreisschnittpunkt. Bei der gewählten Definition der Kreise ist die erste Lösung in der Fünfgelenkkette und die zweite außen (für die gewählten Beispiel-Parameter).
# Die zweite Lösung entspricht der gewählten Lösung in [Bejaoui2018_S749]
r_s1x:=simplify(A(1,2),trig):
r_s1y:=simplify(A(2,2),trig):
# Zwangsbedingungen für eta, zweiter Weg zum Kreisschnittpunkt D über B
radx1:=AB+BC*cos(qJ2s)+CD*(cos(qJ2s)*cos(eta_s)-sin(qJ2s)*sin(eta_s)):
rady1:=BC*sin(qJ2s)+CD*(sin(qJ2s)*cos(eta_s)+cos(qJ2s)*sin(eta_s)):
f11:=r_s1x-radx1=0: #GL3
;
f12:=r_s1y-rady1=0: #GL4
;
# Zwangsbedingungen für xi , zwei Wege Punkt E
racx1:=AE*cos(qJ1s):
racy1:=AE*sin(qJ1s):
racx2:=AB+BC*cos(qJ2s)+CD*(cos(qJ2s)*cos(eta_s)-sin(qJ2s)*sin(eta_s))+ED*(cos(qJ2s)*(cos(eta_s)*cos(xi_s)-sin(eta_s)*sin(xi_s))-sin(qJ2s)*(cos(xi_s)*sin(eta_s)+cos(eta_s)*sin(xi_s))):
racy2:=BC*sin(qJ2s)+CD*(sin(qJ2s)*cos(eta_s)+cos(qJ2s)*sin(eta_s))+ED*(sin(qJ2s)*(cos(eta_s)*cos(xi_s)-sin(eta_s)*sin(xi_s))+cos(qJ2s)*(cos(xi_s)*sin(eta_s)+cos(eta_s)*sin(xi_s))):
f21:=racx1-racx2=0:  # GL5
;
f22:=racy1-racy2=0:  # GL6
;
# Bestimme Winkel
# eta bestimmen
sol1:=solve({f11,f12},{cos(eta_s),sin(eta_s)}):
assign(sol1):
cos_eta_s:=simplify(rhs(sol1[1]),trig):
sin_eta_s:=simplify(rhs(sol1[2]),trig):
# xi bestimmen
sol2:=solve({f21,f22},{cos(xi_s),sin(xi_s)}):
sol2:=op(sol2):
cos_xi_s:=simplify(rhs(sol2[1]),trig):
sin_xi_s:=simplify(rhs(sol2[2]),trig):
# rho bestimmen
cos_psi_phi:=cos(qJ2s)*cos(qJ1s)+sin(qJ2s)*sin(qJ1s): # cos(psi-phi)
;
cos_eta_xi:=cos_eta_s*cos_xi_s-sin_eta_s*sin_xi_s: # cos(eta+xi)
;
sin_psi_phi:=sin(qJ2s)*cos(qJ1s)-cos(qJ2s)*sin(qJ1s): # sin(psi-phi)
;
sin_eta_xi:=sin_eta_s*cos_xi_s+cos_eta_s*sin_xi_s: #sin(eta+xi)
;
cos_rho_s:=cos_psi_phi*cos_eta_xi-sin_psi_phi*sin_eta_xi:
sin_rho_s:=sin_psi_phi*cos_eta_xi+cos_psi_phi*sin_eta_xi:
# Cos-und Sinus-Terme zuweisen

kintmp_subsexp(1,2):=sin_rho_s:
kintmp_subsexp(2,2):=cos_rho_s:
kintmp_subsexp(3,2):=sin_eta_s:
kintmp_subsexp(4,2):=cos_eta_s:
kintmp_subsexp(5,2):=sin_xi_s:
kintmp_subsexp(6,2):=cos_xi_s:
# Terme vereinfachen

if use_simplify>=1 then # bringt leider nichts
  tmp_t0 := time():
  tmp_l0 := length(kintmp_subsexp):
  printf("%s: Vereinfache Ersetzungsausdrücke für Zwangsbedingungen. Länge: %d.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), tmp_l0):
  for i from 1 to RowDimension(kintmp_subsexp) do
    tmp_t1 := time():
    tmp_l1 := length(kintmp_subsexp(i,2)): # es kann sowieso nur einer der beiden Informationen enthalten
    printf("%s: Vereinfache Ersetzungsausdruck %d (%s). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i, String(kintmp_subsexp(i,1)), tmp_l1):
    kintmp_subsexp(i,2) := simplify2(kintmp_subsexp(i,2)):
    tmp_t2 := time():
    tmp_l2 := length(kintmp_subsexp(i,2)):
    printf("%s: Ersetzungsausdruck %d (%s) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i, String(kintmp_subsexp(i,1)), tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end do:
  tmp_l3 := length(kintmp_subsexp):
  printf("%s: Ersetzungsausdrücke vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), tmp_l0, tmp_l3, tmp_t2-tmp_t0):
end if:
# Winkel zuweisen:
kintmp_qs(1,1):=%arctan(sin_rho_s,cos_rho_s):
kintmp_qs(2,1):=%arctan(sin_eta_s,cos_eta_s):
kintmp_qs(3,1):=%arctan(sin_xi_s,cos_xi_s):
# Exportiere Code für folgende Skripte
#
# Die Annahmen sind im Ausdruck bereits in den Variablen gespeichert. Ersetze das "~"-Zeichen.
# Quelle: http://www.mapleprimes.com/questions/207601-Remove-Assumptions
winkel := <<sin_xi|sin(xi_s)>; <cos_xi|cos(xi_s)>; <sin_rho | sin(rho_s)>; <cos_rho | cos(rho_s)>;<sin_eta | sin(eta_s)>; <cos_eta | cos(eta_s)>>:
save winkel, sprintf("../codeexport/fivebar1TE/tmp/kinematic_constraints_maple_inert"):
kintmp_qt := convert_s_t(kintmp_qs):
# Speichere Maple-Ausdruck (Eingabe-Format und internes Format)
save kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_subsexp_maple", robot_name):
save kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_subsexp_maple.m", robot_name):
printf("%s. Ausdrücke für kintmp_subsexp gespeichert (Maple). CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
for i from 1 to RowDimension(kintmp_s) do
  tmp := kintmp_qs(i):
  save tmp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert_kintmpq_%d", robot_name, i):
  save tmp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert_kintmpq_%d.m", robot_name, i):
end do:
save kin_constraints_exist, kintmp_qs, kintmp_qt,kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert", robot_name):
save kin_constraints_exist, kintmp_qs, kintmp_qt, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
printf("%s. Ausdrücke mit Inert-Arctan exportiert (Matlab). CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Liste mit abhängigen konstanten Kinematikparametern erstellen (wichtig für Matlab-Funktionsgenerierung)
read "../helper/proc_list_constant_expressions";
kc_symbols := Matrix(list_constant_expressions( kintmp_subsexp ));
save kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_maple", robot_name):
MatlabExport(kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_matlab.m",robot_name),2);
printf("%s. Zwangsbedingungen berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):

#


# Calculate Jacobi-Matrix based on MDH-Frames
# Introduction
# Jacobi-Matrix für Roboterkinematik basierend auf MDH-Parametern berechnen.
# Die Berechnung erfolgt durch symbolische Ableitung der Transformationsmatrizen für jeden Körper
#
# robot -> Berechnung für allgemeinen Roboter
# tree -> Berechnung für eine beliebige Baumstruktur (ohne Schleifen)
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# jacobian -> Berechnung der Jacobi-Matrix
# baseframe -> Berechnung der Jacobi-Matrix bezogen auf das Basis-Koordinatensystem des Roboters (nicht: Welt-KS)

# Tests
#
# Autor
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-10
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
#
# Sources
# [SchapplerTapOrt2019] Schappler, M. et al.: Resolution of Functional Redundancy for 3T2R Robot Tasks using Two Sets of Reciprocal Euler Angles, Proc. of the 15th IFToMM World Congress, 2019
# [Ortmaier2014] Vorlesungsskript Robotik I
# [Ortmaier2014a] Vorlesungsskript Robotik II
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(ArrayTools):
with(codegen):
with(CodeGeneration):
with(StringTools):
codegen_act := true:
codegen_debug := false:
codegen_opt := 2:
codegen_jacobi := 3: # 1=nur Position, 2=Rotationsmatrix-Jacobi, 3=mit Zeitableitung
;
# Funktionen aus Robotik-Repo
read("../robotics_repo_path"): # Lädt Variable robotics_repo_path
;
read(sprintf("%s/transformation/maple/proc_r2eulxyz_inert", robotics_repo_path)):
# Funktionen aus diesem Repo
read sprintf("../helper/proc_MatlabExport"):
read sprintf("../transformation/proc_transl"):
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
# Roboter-Definitionen
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
# Ergebnisse der Kinematik laden
read sprintf("../codeexport/%s/tmp/kinematics_floatb_%s_rotmat_maple.m", robot_name, base_method_name):
Trf := Trf:
Trf_c := Trf_c:
# Eliminiere die Basis-Position und Orientierung. Darf keinen Einfluss auf die Jacobi-Matrix haben.
# Bei komplizierten Ausdrücken (insbesondere bei kinematischen Zwangsbedingungen) können die Symbole manchmal nicht eliminiert werden
for i from 1 to NQB do
  Trf_c := subs({X_base_t[i,1]=0},Trf_c):
end do:
# Link-Index, für den die Jacobi-Matrix aufgestellt wird. Hier wird angenommen, dass der Endeffektor das letzte Segment (=Link) ist. Die Jacobi-Matrix kann hier aber für beliebige Segmente aufgestellt werden. (0=Basis)
LIJAC:=NL-1:
printf("Generiere Jacobi-Matrix für %s (Körper %d)\n", robot_name, LIJAC):
# Jacobi-Matrix analytisch (Translatorisch)
# Ortmaier2014a Gl. (1.15), S.14: Geometrische Zwangsbedingungen in impliziter Form
# Gleichung enthält verallgemeinerte Koordinaten und Endeffektorposition und ergibt Null.
x_EE := Matrix(3,1,<r_xEE; r_yEE; r_zEE>):
# Transformationsmatrix von der Basis zum gegebenen Punkt auf dem Körper. Die Koordinaten px,py,pz für den Punkt werden später eingesetzt.
T_p := Trf_c(1 .. 4, 1..4, LIJAC+1) . transl(<px;py;pz>):
h_transl := Matrix(T_p(1..3,4)) - x_EE:
h_transl_s :=convert_t_s(h_transl):
# Jacobi-Matrix der inversen Kinematik
b_transl := Matrix(3, NQJ):
for i from 1 to 3 do
  for j from 1 to NQJ do
    b_transl(i,j) := diff(h_transl_s(i,1), qJ_s(j,1)):
  end do:
end do:
# Export
save b_transl, sprintf("../codeexport/%s/tmp/jacobia_transl_%d_maple.m", robot_name, LIJAC):
# Ausdruck muss nochmal geladen werden, ansonsten hängt sich die Code-Optimerung mit "tryhard" auf.
# TODO: Klären warum das so ist und Problem beheben.
read sprintf("../codeexport/%s/tmp/jacobia_transl_%d_maple.m", robot_name, LIJAC):
b_transl := b_transl:
if codegen_act and codegen_debug then
  for i from 1 to 3 do
    for j from 1 to NQJ do
      MatlabExport(b_transl(i,j), sprintf("../codeexport/%s/tmp/jacobia_transl_%d_floatb_%s_%d_%d_matlab.m", robot_name, LIJAC, base_method_name, i, j), codegen_opt):
    end do:
  end do:
end if:
if codegen_act then
  MatlabExport(b_transl, sprintf("../codeexport/%s/tmp/jacobia_transl_%d_floatb_%s_matlab.m", robot_name, LIJAC, base_method_name), codegen_opt):
end if:
# Jacobi-Matrix analytisch (Rotatorisch)
# Ortmaier2014a Gl. (1.15), S.14: Geometrische Zwangsbedingungen in impliziter Form
# Rotationsdarstellung des Endeffektors in RPY-Winkeln
xo_EE := Matrix(3,1,<phi_xEE; phi_yEE; phi_zEE>):
# r2eulxyz_inert liefert inert-arctan (%). Auswertung vor Export dauert zu lange. In exportiertem Matlab-Code muss "%arctan" händisch gegen "atan2" getauscht werden.
h_rota_rpy := r2eulxyz_inert(T_p) - xo_EE:
h_rota_s :=convert_t_s(h_rota_rpy):
# Jacobi-Matrix der inversen Kinematik
# Falls die RPY-Darstellung für dieses Körper-KS singulär ist, existiert die analytische Jacobi-Matrix nicht. Maple gibt dann einen Fehler aus. Die Kennzeichnung mit NaN sorgt dafür, dass dies in Matlab erkennbar ist.
# Die Singularität kann bei kinematischen Strukturen am Anfang auftreten.
b_rota := Matrix(3, NQJ):
for i from 1 to 3 do
  for j from 1 to NQJ do
    if T_p(3,3) = 0 then
      b_rota(i,j) := NaN: # Singulär
    else
      b_rota(i,j) := diff(h_rota_s(i,1), qJ_s(j,1)):
    end if:
  end do:
end do:
# Export
save b_rota, sprintf("../codeexport/%s/tmp/jacobia_rot_%d_maple.m", robot_name, LIJAC):
# Ausdruck nochmal laden.
read sprintf("../codeexport/%s/tmp/jacobia_rot_%d_maple.m", robot_name, LIJAC):
b_rota := b_rota:
# Einzelne Ausdrücke als Code exportieren (falls Gesamtausdruck zu komplex ist)
if codegen_act and codegen_debug then
  for i from 1 to 3 do
    for j from 1 to NQJ do
      MatlabExport(b_rota(i,j), sprintf("../codeexport/%s/tmp/jacobia_rot_%d_floatb_%s_%d_%d_matlab.m", robot_name, LIJAC, base_method_name, i, j), codegen_opt):
    end do:
  end do:
end if:
if codegen_act then
  interface(warnlevel=0): # Unterdrücke die Warnung für arctan
  MatlabExport(b_rota, sprintf("../codeexport/%s/tmp/jacobia_rot_%d_floatb_%s_matlab.m", robot_name, LIJAC, base_method_name), codegen_opt):
  interface(warnlevel=3):
end if:
# Jacobi-Matrix geometrisch (Rotatorisch)
# Zusammenhang zwischen Geschwindigkeit der verallgemeinerten Koordinaten und Winkelgeschwindigkeit des Endeffektors ausgedrückt im Basis-Koordinatensystems
read sprintf("../codeexport/%s/tmp/velocity_worldframe_floatbase_%s_par1_maple.m", robot_name, base_method_name):
omega_W_i := omega_W_i:
# Basis-Terme entfernen (falls durch Maple später nicht möglich, s.o.)
for i from 1 to NQB do
  omega_W_i := subs({X_base_t[i,1]=0},omega_W_i):
  omega_W_i := subs({V_base_t[i,1]=0},omega_W_i):
end do:
# Winkelgeschwindigkeit als allgemeiner Ausdruck (nicht über Gelenk-Konfiguration)
# (fällt später sowieso weg)
omega_EE := Matrix(3,1,<omega_xEE; omega_yEE; omega_zEE>):
# Differenz als kinematische Zwangsbedingung aufstellen
h_rotg := Matrix(omega_W_i(1..3,LIJAC+1)) - omega_EE:
h_rotg_s :=convert_t_s(h_rotg):
# Jacobi-Matrix der inversen Kinematik
# Leite die Winkelgeschwindigkeiten des Endeffektors nach den Gelenkwinkelgeschwindigkeiten ab.
# Damit wird der Zusammenhang nach Ortmaier2014 Gl. (4.17), S.51 umgeformt.
b_rotg := Matrix(3, NQJ):
for i from 1 to 3 do
  for j from 1 to NQJ do
    b_rotg(i,j) := diff(h_rotg_s(i,1), qJD_s(j,1)):
  end do:
end do:
B_s := b_rotg:
# Export
save b_rotg, sprintf("../codeexport/%s/tmp/jacobig_rot_%d_maple.m", robot_name, LIJAC):
# Ausdruck nochmal laden.
read sprintf("../codeexport/%s/tmp/jacobig_rot_%d_maple.m", robot_name, LIJAC):
b_rotg := b_rotg:
if codegen_act and codegen_debug then
  for i from 1 to 3 do
    for j from 1 to NQJ do
      MatlabExport(b_rotg(i,j), sprintf("../codeexport/%s/tmp/jacobig_rot_%d_floatb_%s_%d_%d_matlab.m", robot_name, LIJAC, base_method_name, i, j), codegen_opt):
    end do:
  end do:
end if:
if codegen_act then
  MatlabExport(b_rotg, sprintf("../codeexport/%s/tmp/jacobig_rot_%d_floatb_%s_matlab.m", robot_name, LIJAC, base_method_name), codegen_opt):
end if:
# Jacobi-Matrix der Rotationsmatrix (Rotatorisch)
# Ableitung der Rotationsmatrix nach den Gelenkwinkel (kann z.B. für die inverse Kinematik benutzt werden)
# Siehe [SchapplerTapOrt2019], Gl. 31, Term III und Gl. 27
R_0_i_s := convert_t_s( Trf_c(1 .. 3, 1..3, LIJAC+1) ):
Rb_0_i_s :=Reshape(R_0_i_s, 9,1):
J_Rb_s := Matrix(9, NQJ):
for i from 1 to 9 do
  for j from 1 to NQJ do
    J_Rb_s(i,j) := diff(Rb_0_i_s(i,1), qJ_s(j,1)):
  end do:
end do:
save J_Rb_s, sprintf("../codeexport/%s/tmp/jacobiR_rot_%d_floatb_%s_maple.m", robot_name, LIJAC, base_method_name):
if codegen_act and codegen_jacobi > 1 then
  MatlabExport(J_Rb_s, sprintf("../codeexport/%s/tmp/jacobiR_rot_%d_floatb_%s_matlab.m", robot_name, LIJAC, base_method_name), codegen_opt):
end if:
# Jacobi-Zeitableitungen
# Die Jacobi-Zeitableitungen werden z.B. zur Umrechnung zwischen EE-Beschleunigungen und Gelenk-Beschleunigungen benötigt.
JD_rotg_t := diff~(convert_s_t(b_rotg), t):
JD_rotg_s := convert_t_s(JD_rotg_t):
JD_rota_t := diff~(convert_s_t(b_rota), t):
JD_rota_s := convert_t_s(JD_rota_t):
JD_transla_t := diff~(convert_s_t(b_transl), t):
JD_transla_s := convert_t_s(JD_transla_t):
# Zeitableitung der Ableitung der Rotationsmatrix nach den Gelenkwinkeln. Dieser Term wird zur Berechnung der Zeitableitung der kinematischen Zwangsbedingungen für PKM benötigt.
# Das wird bei der Berechnung der Coriolis-Terme aus dieser Darstellung benötigt.
JD_Rb_t := diff~(convert_s_t(J_Rb_s), t):
JD_Rb_s := convert_t_s(JD_Rb_t):
# Matlab-Export
if codegen_act and codegen_jacobi > 2 then
  MatlabExport(JD_rotg_s, sprintf("../codeexport/%s/tmp/jacobigD_rot_%d_floatb_%s_matlab.m", robot_name, LIJAC, base_method_name), codegen_opt):
  interface(warnlevel=0): # Unterdrücke die Warnung für arctan
  MatlabExport(JD_rota_s, sprintf("../codeexport/%s/tmp/jacobiaD_rot_%d_floatb_%s_matlab.m", robot_name, LIJAC, base_method_name), codegen_opt):
  interface(warnlevel=3):
  MatlabExport(JD_transla_s, sprintf("../codeexport/%s/tmp/jacobiaD_transl_%d_floatb_%s_matlab.m", robot_name, LIJAC, base_method_name), codegen_opt):
  MatlabExport(JD_Rb_s, sprintf("../codeexport/%s/tmp/jacobiRD_rot_%d_floatb_%s_matlab.m", robot_name, LIJAC, base_method_name), codegen_opt):
end if:


(*
 Copyright (C) 2017 M.A.L. Marques
 Copyright (C) 2018 Susi Lehtola

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_exc *)
(* prefix:
  mgga_x_rtpss_params *params;

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

(* These are used within the tpss_x routine *)
tpss_ff     := z -> 2:
tpss_kappa := (x, t) -> params_a_kappa:

$include "tpss_x.mpl"

(* Equation (6) *)

rtpss_f := (x, u, t) -> 1 + tpss_kappa(x, t)*(1 - exp(-tpss_fx(x, t)/tpss_kappa(x,t))):

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


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: work_mgga_c *)

$define lda_c_pw_params
$include "lda_c_pw.mpl"

$define lda_x_params
$include "lda_x.mpl"

gamma_x  := 0.11:
gamma_ss := 1.6:
gamma_ab := 0.14:

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

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

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

f_b98 := (lda_func, g_ss, cc_ss, g_ab, cc_ab, rs, z, xs0, xs1, ts0, ts1, us0, us1) ->
  + lda_stoll_par(lda_func, rs,  z,  1)
    * b98_g(g_ss, cc_ss, b98_q(xs0, ts0, us0)) * Fermi_D(xs0, ts0)
  + lda_stoll_par(lda_func, rs, -z, -1)
    * b98_g(g_ss, cc_ss, b98_q(xs1, ts1, us1)) * Fermi_D(xs1, ts1)
  + lda_stoll_perp(lda_func, rs, z)
    * b98_g(g_ab, cc_ab, (b98_q(xs0, ts0, us0) + b98_q(xs1, ts1, us1))/2):

f := (rs, z, xt, xs0, xs1, ts0, ts1, us0, us1) ->
  + f_lda_x(rs,  1)*b98_g(gamma_x, params_a_c_x, b98_q(xs0, ts0, us0))
  + f_lda_x(rs, -1)*b98_g(gamma_x, params_a_c_x, b98_q(xs1, ts1, us1))
  + f_b98(f_pw, gamma_ss, params_a_c_ss, gamma_ab, params_a_c_ab,
        rs, z, xs0, xs1, ts0, ts1, us0, us1):


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)

g96_c1 := 1/137:

f_g96  := x->
  1 + g96_c1/X_FACTOR_C * x^(3/2):

f  := (rs, z, xt, xs0, xs1) -> gga_exchange(f_g96, rs, z, xs0, xs1):


#######################################################################
# This file is part of the crlibm library, and is distributed under
# the  LGPL.
# To use:
# restart; read "log2-td.mpl";
Digits := 120:

interface(quiet=true):

read "common-procedures.mpl":
read "triple-double.mpl":
mkdir("TEMPLOG"):

# We want log2h + log2m + log2l + delta = log(2) such that
# log2h and log2m have at least 11 trailing zeros
# in order to have an exact multiplication with E, which is lower than 1024 in
# magnitude
# The resting accuracy is enough for both quick and accurate phases.

log2acc := log(2):
log2h := round(log2acc * 2**(floor(-log[2](abs(log2acc))) + (53 - 11))) / 2**(floor(-log[2](abs(log2acc))) + (53 - 11)):
log2m := round((log2acc - log2h) * 2**(floor(-log[2](abs((log2acc - log2h)))) + (53 - 11))) / 2**(floor(-log[2](abs((log2acc - log2h)))) + (53 - 11)):
log2l := log2acc - (log2h + log2m):

L := 7: # number of bits used to address the table

MAXINDEX    := round(2^L * (sqrt(2)-1)):

for i from 0 to MAXINDEX-1 do
    center[i] := 1 + i*2^(-L): # center[i] in [1, 2[
    t :=  evalf(1/center[i]):
    r[i] := round(t * 2**(floor(-log[2](abs(t))) + 23)) / 2**(floor(-log[2](abs(t))) + 23):
    (logih[i], logim[i], logil[i]) := hi_mi_lo(evalf(-log(r[i]))):
od:
for i from MAXINDEX to 2^L do
    # y has been divided by two, center[i] in [0.5, 1[
    center[i]:=(1 + i*2^(-L)) / 2:
    t :=  evalf(1/center[i]):
    r[i] := round(t * 2**(floor(-log[2](abs(t))) + 23)) / 2**(floor(-log[2](abs(t))) + 23):
    (logih[i], logim[i], logil[i]) := hi_mi_lo(evalf(-log(r[i]))):
od:

#Computation of ZMax.
for i from 0 to MAXINDEX-1 do
    __x := center[i] + 2^(-L-1) :
    zmax[i] := (__x*r[i]-1) :
    __x := center[i] - 2^(-L-1) :
    zmin[i] := (__x*r[i]-1) :
od:
for i from MAXINDEX to 2^L do
    __x := center[i] + 2^(-L-2) :
    zmax[i] := (__x*r[i]-1) :
    __x := center[i] - 2^(-L-2) :
    zmin[i] := (__x*r[i]-1) :
od:

zmaxmax:=0:
zminmin:=0:
for i from 0 to 2^L do
    tabulated_value := logih[i] + logim[i] + logil[i]:
    poly_approx_min := evalf(log(1+zmin[i])):
    poly_approx_max := evalf(log(1+zmax[i])):

    # Test if we have a case where we cancellate a lot
    # i.e. the polynomial approximation value is greater than half the tabulated value
    # the tabulated value is not exactly zero and we are of opposite sign

    if ((abs(poly_approx_min) > 0.75*abs(tabulated_value)) and (tabulated_value <> 0.0) and (poly_approx_min * tabulated_value < 0)) then
	printf("Polynomial approximation is greater in magnitude in zmin[%d] than half the tabluated value\n",i):

	printf("The tabulated value is %1.50e\n",tabulated_value):
	if (tabulated_value <> 0.0) then printf("i.e. the value has the exponent %d\n",floor(log2(abs(tabulated_value)))) fi:
	printf("The value of polynomial in zmin[%d] is %1.50e\n",i,poly_approx_min):
        if (poly_approx_min <> 0.0) then printf("i.e. the value has the exponent %d\n",floor(log2(abs(poly_approx_min)))) fi:

	summe := poly_approx_min + tabulated_value:
	printf("The exponent of the sum of both is %d\n",floor(log2(abs(summe)))):

    fi:

    if ((abs(poly_approx_max) > 0.75*abs(tabulated_value)) and (tabulated_value <> 0.0) and (poly_approx_max * tabulated_value <0)) then
	printf("Polynomial approximation is greater in magnitude in zmax[%d] than half the tabluated value\n",i):

	printf("The tabulated value is %1.50e\n",tabulated_value):
	if (tabulated_value <> 0.0) then printf("i.e. the value has the exponent %d\n",floor(log2(abs(tabulated_value)))) fi:
	printf("The value of polynomial in zmax[%d] is %1.50e\n",i,poly_approx_max):
        if (poly_approx_max <> 0.0) then printf("i.e. the value has the exponent %d\n",floor(log2(abs(poly_approx_max)))) fi:

	summe := poly_approx_max + tabulated_value:
	printf("The exponent of the sum of both is %d\n",floor(log2(abs(summe)))):

    fi:

    if zmax[i] > zmaxmax then zmaxmax := zmax[i]: fi:
    if zmin[i] < zminmin then zminmin := zmin[i]: fi:
od:
printf("zminmin = -2^(%2f)   zmaxmax = 2^(%2f)\n", log2(-zminmin), log2(zmaxmax) ) :

PolyDegreeQuick:=7:

printf("   degree of the polynomial used in the quick phase is %d\n",PolyDegreeQuick);

DDNumberQuick:=3:

printf("   number of double doubles used for the coefficients is %d\n",DDNumberQuick);

#Keep -zmaxmax..zmaxmax to keep c1=1, which is useful in the proof
#and constrain the first two coefficients to 1 and -1/2 in order to save up a full multiplication and a rounding error
polyQuick:= poly_exact2(x*(1+x*(-0.5+x*(numapprox[minimax]((((log(1+x)/x)-1)/x+0.5)/x,
			x=-zmaxmax..zmaxmax,  [PolyDegreeQuick-3,0], 1 ,  'deltaApprox')))), DDNumberQuick):

#Try to verify the bound for using double double arithmetic.
#Idea: compare the maximum absolute value of the polynomial in zmaxmax (the polynomial has its maxima at the limits)
#with the maximum value of the first term which is calculated in double precision only

p := unapply(polyQuick,x):
printf("   using only %d double doubles should be fine since the hi z ulp should affect the result starting from bit %f\n",
	DDNumberQuick,evalf(53 + log2(p(zmaxmax)) - log2(zmaxmax^(DDNumberQuick)),5)):

epsilonApproxQuick := numapprox[infnorm]( 1-polyQuick/log(1+x), x=zminmin..zmaxmax):
printf("   approximation rel error for the quick phase is 2^(%2f)\n", log2(epsilonApproxQuick) ) :
deltaApproxQuick := numapprox[infnorm]( polyQuick-log(1+x), x=zminmin..zmaxmax):
printf("   approximation abs error for the quick phase is 2^(%2f)\n", log2(deltaApproxQuick) ) :

PolyDegreeAccurate:=14:

printf("   degree of the polynomial used in the accurate phase is %d\n",PolyDegreeAccurate);

DDNumberAccu:=7:
TDNumberAccu:=3:

printf("   number of triple doubles used for the coefficients is %d\n",TDNumberAccu);
printf("   number of double doubles used for the coefficients is %d\n",DDNumberAccu);

#Keep -zmaxmax..zmaxmax to keep c1=1, which is useful in the proof
polyAccurate:= poly_exact32(x*(1+x*(-0.5+x*(numapprox[minimax]((((log(1+x)/x)-1)/x+0.5)/x,
				x=-zmaxmax..zmaxmax,  [PolyDegreeAccurate-3,0], 1 ,  'deltaApprox')))),
				TDNumberAccu, DDNumberAccu):

#Try to verify the bound for using double double arithmetic.
#Idea: compare the maximum absolute value of the polynomial in zmaxmax (the polynomial has its maxima at the limits)
#with the maximum value of the first term which is calculated in double precision only

pp := unapply(polyAccurate,x):
printf("   using only %d triple doubles should be fine since the mi z ulp should affect the result starting from bit %f\n",
	TDNumberAccu,evalf(106 + log2(p(zmaxmax)) - log2(zmaxmax^(TDNumberAccu)),5)):
printf("   using only %d double doubles should be fine since the hi z ulp should affect the result starting from bit %f\n",
	DDNumberAccu,evalf(53 + log2(p(zmaxmax)) - log2(zmaxmax^(TDNumberAccu + DDNumberAccu)),5)):

epsilonApproxAccurate := numapprox[infnorm]( 1-polyAccurate/log(1+x), x=zminmin..zmaxmax):
printf("   approximation rel error for the accurate phase is 2^(%2f)\n", log2(epsilonApproxAccurate) ) :
deltaApproxAccurate := numapprox[infnorm]( polyAccurate-log(1+x), x=zminmin..zmaxmax):
printf("   approximation abs error for the quick phase is 2^(%2f)\n", log2(deltaApproxAccurate) ) :

#Compute now the inverse of ln(10) for the final multiplication of ln(x) with this constant for obtaining log10(x)
#Compute also the relative error of the constant stored as a triple double.
Log10inv := evalf(1 / log(10)):

(log10invh, log10invm, log10invl) := hi_mi_lo(Log10inv):

Log10invhml := log10invh + log10invm + log10invl:

epsilonLog10invhml := evalf(abs((Log10invhml - Log10inv) / Log10inv)):

printf("   Log10inv = 1 / ln(10) stored as a triple-double is exact with a relative error of 2^(%2f)\n",
	evalf(log[2](epsilonLog10invhml))):

#Compute now the constant needed for the unfiltered directed final rounding of the triple-double result
#This constant is supposed to be the reciprocal of the critical accuracy of the function.
#We suppose this critical accuracy to be 2^(-120) because the worst case (for RD) we know of is x = 403ce41d 8fa665fa
#We can easily spend some guard bits since we want simply filter out cases with a theoretical worst case accuracy
#of -infty (exact floating point images) and as we know that we are exact to at least 122 bits.

wca := 2^(122):

#-------------------------------------------------------------------
# Output

filename:="TEMPLOG/log10-td.h":
fd:=fopen(filename, WRITE, TEXT):

fprintf(fd, "#include \"crlibm.h\"\n#include \"crlibm_private.h\"\n"):

fprintf(fd, "\n/*File generated by maple/log10-td.mpl*/\n"):

fprintf(fd, "\n\#define L %d\n\n",L):
fprintf(fd, "\#define MAXINDEX %d\n\n", MAXINDEX):
fprintf(fd, "\#define INDEXMASK %d\n", 2^L-1):
fprintf(fd, "\#define two52 %1.50e\n", 2^(52)):
fprintf(fd, "\#define log2h %1.50e\n", log2h):
fprintf(fd, "\#define log2m %1.50e\n", log2m):
fprintf(fd, "\#define log2l %1.50e\n", log2l):
fprintf(fd, "\#define log10invh %1.50e\n",log10invh):
fprintf(fd, "\#define log10invm %1.50e\n",log10invm):
fprintf(fd, "\#define log10invl %1.50e\n",log10invl):
fprintf(fd, "\#define WORSTCASEACCURACY %1.50e\n",wca):

epsilon_quick_1 := 2^(-61): # The Gappa proof will show this bound
epsilon_quick_2 := 2^(-61): # The Gappa proof will show this bound
fprintf(fd, "\#define ROUNDCST1 %1.50e\n", compute_rn_constant(epsilon_quick_1)):
fprintf(fd, "\#define ROUNDCST2 %1.50e\n", compute_rn_constant(epsilon_quick_2)):
fprintf(fd, "\#define RDROUNDCST1 %1.50e\n", epsilon_quick_1):
fprintf(fd, "\#define RDROUNDCST2 %1.50e\n", epsilon_quick_2):

fprintf(fd, "\n\n"):

# Print the defines for the define statements

for i from 3 to PolyDegreeQuick do
	fprintf(fd, "\#define c%d %1.50e\n",i,coeff(polyQuick,x,i)):
od:

fprintf(fd, "\n\n"):

for i from 3 to (DDNumberAccu + TDNumberAccu -1) do
	(hi,lo) := hi_lo(coeff(polyAccurate,x,i)):
	fprintf(fd, "\#define accPolyC%dh %1.50e\n",i,hi):
	fprintf(fd, "\#define accPolyC%dl %1.50e\n",i,lo):
od:

for i from (DDNumberAccu + TDNumberAccu) to PolyDegreeAccurate do
	fprintf(fd, "\#define accPolyC%d %1.50e\n",i,coeff(polyAccurate,x,i)):
od:

fprintf(fd, "\n\n"):

# Print the table
fprintf(fd, "typedef struct rri_tag {float ri; double logih; double logim; double logil;} rri;  \n"):
fprintf(fd, "static const rri argredtable[%d] = {\n", 2^L):
for i from 0 to 2^L-1 do
      fprintf(fd, "  { \n"):
      fprintf(fd, "    %1.50e,   /* r[%d] */ \n", r[i], i):
      fprintf(fd, "    %1.50e, /* logih[%d] */ \n", logih[i], i):
      fprintf(fd, "    %1.50e, /* logim[%d] */ \n", logim[i], i):
      fprintf(fd, "    %1.50e, /* logil[%d] */ \n", logil[i], i):
      fprintf(fd, "  } "):
      if(i<2^L-1) then  fprintf(fd, ", \n"): fi
od:
fprintf(fd, "}; \n \n"):

fclose(fd):

for j from 0 to 2^L-1 do
    filename:=cat("TEMPLOG/log10-td_",j,".sed"):
    fd:=fopen(filename, WRITE, TEXT):
    fprintf(fd, " s/_log2h/%1.50e/g\n", log2h):
    fprintf(fd, " s/_log2m/%1.50e/g\n", log2m):
    fprintf(fd, " s/_log2l/%1.50e/g\n", log2l):
    fprintf(fd, " s/_logih/%1.50e/g\n", logih[j]):
    fprintf(fd, " s/_logim/%1.50e/g\n", logim[j]):
    fprintf(fd, " s/_logil/%1.50e/g\n", logil[j]):
    fprintf(fd, " s/_zmin/%1.50e/g\n", zmin[j]):
    fprintf(fd, " s/_zmax/%1.50e/g\n", zmax[j]):
    for i from 3 to PolyDegreeQuick do
        fprintf(fd, " s/_c%d/%1.50e/g\n", i, coeff(polyQuick,x,i)):
    od:
    fprintf(fd, " s/_epsilonApproxQuick/%1.50e/g\n", epsilonApproxQuick):
    fprintf(fd, " s/_epsilonLog10invhml/%1.50e/g\n", epsilonLog10invhml):
    fclose(fd):
  od:

for j from 0 to 2^L-1 do
    filename:=cat("TEMPLOG/log10-td-accurate_",j,".sed"):
    fd:=fopen(filename, WRITE, TEXT):
    fprintf(fd, " s/_log2h/%1.50e/g\n", log2h):
    fprintf(fd, " s/_log2m/%1.50e/g\n", log2m):
    fprintf(fd, " s/_log2l/%1.50e/g\n", log2l):
    fprintf(fd, " s/_logih/%1.50e/g\n", logih[j]):
    fprintf(fd, " s/_logim/%1.50e/g\n", logim[j]):
    fprintf(fd, " s/_logil/%1.50e/g\n", logil[j]):
    fprintf(fd, " s/_zmin/%1.50e/g\n", zmin[j]):
    fprintf(fd, " s/_zmax/%1.50e/g\n", zmax[j]):
    for i from 3 to (DDNumberAccu + TDNumberAccu -1) do
	(hi,lo) := hi_lo(coeff(polyAccurate,x,i)):
        fprintf(fd, " s/_accPolyC%dh/%1.50e/g\n", i, hi):
        fprintf(fd, " s/_accPolyC%dl/%1.50e/g\n", i, lo):
    od:
    for i from (DDNumberAccu + TDNumberAccu) to PolyDegreeAccurate do
        fprintf(fd, " s/_accPolyC%d/%1.50e/g\n", i, coeff(polyAccurate,x,i)):
    od:
    fprintf(fd, " s/_epsilonApproxAccurate/%1.50e/g\n", epsilonApproxAccurate):
    fprintf(fd, " s/_epsilonLog10invhml/%1.50e/g\n", epsilonLog10invhml):
    fclose(fd):
  od:

# A shell script to use them
filename:="run-log10-td-proof.sh":
fd:=fopen(filename, WRITE, TEXT):
fprintf(fd, "#!/bin/sh\n"):
fprintf(fd, "# You probably need to edit the path to the gappa executable\n"):
fprintf(fd, "GAPPA=~/ble/gappa-0.4.5/src/gappa\n"):
fprintf(fd, "# Test all the possible table value for E=1\n"):
fprintf(fd, "for num in `seq 0 %d`; do\n", 2^L-1):
fprintf(fd, "  echo $num, E=1:\n"):
fprintf(fd, "  sed -f ./TEMPLOG/log10-td_$num.sed log10-td.gappa | $GAPPA > /dev/null\n"):
fprintf(fd, "  echo\n"):
fprintf(fd, "done\n"):
fprintf(fd, "# For the case E=0 we first handle the cases 0 and %d using log10-td-E0-logir0.gappa\n", 2^L):
fprintf(fd, "echo 0 and %d, E=0:\n", 2^L):
fprintf(fd, "sed -f log10-td_0.sed log10-td-E0-logir0.gappa | $GAPPA > /dev/null\n"):
fprintf(fd, "# then the other cases where logirh <> 0\n"):
fprintf(fd, "for num in `seq 1 %d`; do\n", 2^L-1):
fprintf(fd, "  echo $num, E=0:\n"):
fprintf(fd, "  sed -f ./TEMPLOG/log10-td_$num.sed log10-td-E0.gappa | $GAPPA > /dev/null\n"):
fprintf(fd, "  echo\n"):
fprintf(fd, "done\n"):
fprintf(fd, "# Accurate phase: Test all the possible table value for E=1\n"):
fprintf(fd, "for num in `seq 0 %d`; do\n", 2^L-1):
fprintf(fd, "  echo Accurate phase: $num, E=1:\n"):
fprintf(fd, "  sed -f ./TEMPLOG/log10-td-accurate_$num.sed log10-td-accurate.gappa | $GAPPA > /dev/null\n"):
fprintf(fd, "  echo\n"):
fprintf(fd, "done\n"):
fprintf(fd, "# Accurate phase: For the case E=0 we first handle the cases 0 and %d using log10-td-accurate-E0-logir0.gappa\n", 2^L):
fprintf(fd, "echo 0 and %d, E=0:\n", 2^L):
fprintf(fd, "sed -f ./TEMPLOG/log10-td-accurate_0.sed log10-td-accurate-E0-logir0.gappa | $GAPPA > /dev/null\n"):
fprintf(fd, "# Accurate phase: then the other cases where logirh <> 0\n"):
fprintf(fd, "for num in `seq 1 %d`; do\n", 2^L-1):
fprintf(fd, "  echo $num, E=0:\n"):
fprintf(fd, "  sed -f ./TEMPLOG/log10-td-accurate_$num.sed log10-td-accurate-E0.gappa | $GAPPA > /dev/null\n"):
fprintf(fd, "  echo\n"):
fprintf(fd, "done\n"):
fclose(fd):

printf("----DONE---\n") :


# Simplicial constructions for a proof of the Hurewicz theorem

`is_element/K/hurewicz` := (A::set) -> (d::posint) -> proc(x)
 if not(`is_element/prime_simplex`(A)(x)) then
  return false;
 fi;

 if nops(select(a -> x[a]>0,A)) > d then
  return false;
 fi;

 return true;
end:

`random_element/K/hurewicz` := (A::set) -> (d::posint) -> proc()
 local k,i,B,x,a;
 k := rand(1..min(nops(A),d))();
 B := {};
 for i from 1 to k do
  B := {op(B),random_element_of(A minus B)};
 od:
 x := `random_element/prime_simplex`(B)();
 for a in A minus B do x[a] := 0; od;
 return eval(x);
end:

`list_elements/K_hurewicz` := NULL:
`count_elements/K_hurewicz` := NULL:

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

`is_element/L/hurewicz` := (A::set) -> (d::posint) -> proc(tx)
 local t,x;

 if not(type(tx,list)) and nops(tx) = 2 then
  return false;
 fi;

 t,x := op(tx);

 if not(type(t,realcons) and is(t >= 0) and is(t <= 1)) then
  return false;
 fi;

 if not(`is_element/prime_simplex`(A)(x)) then
  return false;
 fi;

 if t = 1 then
  return true;
 fi;

 if `is_element/K/hurewicz`(A)(d)(x) then
  return true;
 fi;

 return false;
end:

`random_element/L/hurewicz` := (A::set) -> (d::posint) -> proc()
 if rand(1..2)() = 1 then
  return [1,`random_element/prime_simplex`(A)()];
 else
  return [rand(0..12)()/12,`random_element/K/hurewicz`(A)(d)()];
 fi;
end:

`list_elements/L_hurewicz` := NULL:
`count_elements/L_hurewicz` := NULL:

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

`u1/hurewicz` := (A::set) -> (d::posint) -> proc(x)
 local JJ,J,j;
 if nops(A) <= d then return 0; fi;
 JJ := combinat[choose](A,nops(A) - d);
 return min(seq(max(seq(x[j],j in J)),J in JJ));
end:

`w1/hurewicz` := (A::set) -> (d::posint) -> proc(tx)
 local t,x,u1;
 t,x := op(tx);
 u1 := `u1/hurewicz`(A)(d)(x);
 return min(u1+(1+t)/2,1);
end:

`f1/hurewicz` := (A::set) -> (d::posint) -> proc(tx)
 local t,x,y,a;
 t,x := op(tx);
 y := table():
 for a in A do
  y[a] := max(0,x[a]-(1-t)/2,2*x[a]-1);
 od:
 return eval(y);
end:

`h1/hurewicz` := (A::set) -> (d::posint) -> proc(tx)
 return [`w1/hurewicz`(A)(d)(tx),`f1/hurewicz`(A)(d)(tx)];
end:


with(DifferentialAlgebra):
ring_diff := DifferentialRing(blocks = [[S, A, E, C, Ninv, J, I], [y2, y] ], derivations = [t]):
sys := [
diff(S(t), t) - (-b*S(t)*Ninv(t)*A(t)*q - b*S(t)*Ninv(t)*II(t) - b*S(t)*Ninv(t)*J(t)),
diff(A(t), t) - (-E(t)*k*r + E(t)*k - A(t)*g1),
diff(E(t), t) - (b*S(t)*Ninv(t)*A(t)*q + b*S(t)*Ninv(t)*II(t) + b*S(t)*Ninv(t)*J(t) - E(t)*k),
diff(C(t), t) - (alpha*II(t)),
diff(Ninv(t), t) - (0),
diff(J(t), t) - (alpha*II(t) - g2*J(t)),
diff(II(t), t) - (-alpha*II(t) + E(t)*k*r - g1*II(t)),
y2(t) - (Ninv(t)),
y(t) - (C(t))
];
res := CodeTools[CPUTime](RosenfeldGroebner(sys, ring_diff, singsol=none));


RP2_net_complex := proc()
 local T,rt,tau,i;

 T["vertices"] := {seq(i,i=1..9)};
 T["faces"] := {{1,2,3},{1,2,5},{1,3,9},{1,4,5},{1,4,9},{2,3,7},{2,5,6},{2,6,7},{3,7,8},{3,8,9}};
 T["max_simplices"] := T["faces"];

 rt := sqrt(3);
 tau := (1+sqrt(5))/2;

 T["flat_embedding"] := table([
  1 = [ 1/2, 0   ],
  2 = [-1/4, rt/4],
  3 = [-1/4,-rt/4],
  4 = [ 1  , 0   ],
  5 = [ 1/2, rt/2],
  6 = [-1/2, rt/2],
  7 = [-1  , 0   ],
  8 = [-1/2,-rt/2],
  9 = [ 1/2,-rt/2]
 ]):

 T["icosahedron_embedding"] := table([
   1 = [   0,   1, tau],
   2 = [ tau,   0,   1],
   3 = [   1, tau,   0],
   4 = [-tau,   0,   1],
   5 = [   0,  -1, tau],
   6 = [   1,-tau,   0],
   7 = [ tau,   0,  -1],
   8 = [   0,   1,-tau],
   9 = [  -1, tau,   0]
 ]):

 T["flat_plot"] :=
   `plot/simplicial_complex`(T["vertices"])(T["faces"],2,T["flat_embedding"]);

 T["icosahedron_plot"] :=
   `plot/simplicial_complex`(T["vertices"])(T["faces"],2,T["icosahedron_embedding"]);

 return eval(T);
end:

RP2_complex := proc()
 local T0,T,i;

 T0 := RP2_net_complex();
 T["vertices"] := {seq(i,i=1..6)};
 T["faces"] := subs({7=4,8=5,9=6},T0["faces"]);
 T["max_simplices"] := T["faces"];

 return eval(T);
end:


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: work_mgga_x *)
(* prefix:
  mgga_x_scan_params *params;

  assert(pt->params != NULL);
  params = (mgga_x_scan_params * )(pt->params);
*)

p     := x -> X2S^2*x^2:
alpha := (x, t) -> (t - x^2/8)/K_FACTOR_C:

f_alpha := a -> piecewise(a <= 1, exp(-params_a_c1*a/(1 - a)), -params_a_d*exp(params_a_c2/(1 - a))):

h1x := x -> 1 + params_a_k1*(1 - params_a_k1/(params_a_k1 + x)):

b2 := sqrt(5913/405000):
b1 := (511/13500)/(2*b2):
b3 := 1/2:
b4 := MU_GE^2/params_a_k1 - 1606/18225 - b1^2:
y  := (x, a) -> MU_GE*p(x) + b4*p(x)^2*exp(-b4*p(x)/MU_GE) + (b1*p(x) + b2*(1 - a)*exp(-b3*(1 - a)^2))^2:

a1 := 4.9479:
gx := x -> 1 - exp(-a1/sqrt(X2S*x)):

h0x := 1.174:
f   := (rs, x, t, u) -> (h1x(y(x, alpha(x, t)))*(1 - f_alpha(alpha(x, t))) + h0x*f_alpha(alpha(x, t)))*gx(x):


# 计算不变量阶数
$ifndef _INVORDER_
$define _INVORDER_

(*
    * 计算不变量的次数
*)
findInvariantsOrder:=proc(ee)
    if not type(denom(ee),numeric) then
        return findOrder(numer(ee))-findOrder(denom(ee));
    else
        return findOrder(ee);
    end if;
end proc:

(*
    * 计算齐次表达式的的次数（不含分母）
*)
findOrder:=proc(ee)
    local e;
    e:=expand(ee);
    if type(e,`+`) then
        e:=op(1,e);
    end if;
    e:=remove(type,e,numeric);
    if (e=NULL) then
        return 0;
    end if:
    return findItemOrder(e);
end proc:

(*
    * 计算一项的次数
*)
findItemOrder:=proc(ee)
    local s,_e;
    s:=0;
    for _e in ee do
        if type(_e,`^`) then
            s:=s+op(2,_e);
        else
            s:=s+1;
        end if;
    end do;
end proc:

$endif


# Energy Calculation for the Robot links based on MDH frames
# Introduction
# Berechnung von kinetischer Energie für den Roboter.
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# tree -> Berechnung für eine beliebige Baumstruktur (ohne Schleifen)
# floatb -> floating base wird durch base twist (Geschwindigkeit der Basis) oder vollständige Orientierung (Euler-Winkel) berücksichtigt
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# energy -> Berechnung der Energie
# linkframe -> Berechnung der Geschwindigkeit im Körper-KS (KSi)
# par2 -> Parametersatz 2 (erstes Moment anstelle von Schwerpunkt als Parameter)
# Authors
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-03
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Sources
# [GautierKhalil1988] A direct determination of minimum inertial parameters of robots
# [GautierKhalil1990] Direct Calculation of Minimum Set of Inertial Parameters of Serial Robots
# [KhalilDombre2002] Modeling, Identification and Control of Robots
# [Craig] Introduction to Robotics, Modeling and Control (3rd Edition)
# [Ortmaier2014] Vorlesungsskript Robotik I
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(ArrayTools):
with(codegen):
with(CodeGeneration):
with(StringTools):
codegen_act := true:
codegen_opt := 2:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotx":
read "../transformation/proc_troty":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../transformation/proc_trafo_mdh":
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name, base_method_name):
read sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
kin_constraints_exist := kin_constraints_exist: # Laden zur Einstellung der Term-Vereinfachung
;
# Einstellungen für Term-Vereinfachungen: 0=keine, 2=E_kin (einzelne), 3=auch E_kin (komplett)
if not assigned(simplify_options) or simplify_options(6)=-1 then
  # Die Vereinfachung ist ein Kompromiss aus Laufzeit des Programms und Güte der Ergebnisse
  # Bei zu komplizierten Termen stürzt Maple bei der Vereinfachung unvorhersehbar ab.
  # Die folgenden Regeln sind konservative Einstellungen, damit die Code-Generierung nicht zu lange dauert.
  if not kin_constraints_exist then # ohne Zwangsbedingungen. Annahme: Bringt dann sowieso nicht so viel
    use_simplify := 0: # Keine Vereinfachungen
  else # für Systeme mit Zwangsbedingungen
    if NJ <= 7 then
      use_simplify := 3: # standardmäßig bei einfachen Systemen alle simplify-Befehle anwenden
    else
      use_simplify := 0: # Sonst keine. Optimierung der einzelnen kinetischen Energien bringt sowieso nichts.
    end if:
  end if:
else
  use_simplify := simplify_options(6): # sechster Eintrag ist für Energie
end if:

# Ergebnisse der Kinematik laden
read sprintf("../codeexport/%s/tmp/kinematics_floatb_%s_rotmat_maple.m", robot_name, base_method_name):
Trf_c := Trf_c:
read sprintf("../codeexport/%s/tmp/velocity_linkframe_floatb_%s_maple.m", robot_name, base_method_name):
omega_i_i := omega_i_i:
rD_i_i := rD_i_i:

# Calculate Energy
# Use velocities in link frame and MX,MY,MZ as a parameter
# Kinetic Energy
# [GautierKhalil1988], equ.6
# Hier wird der Trägheitstensor um den Koordinatenursprung gewählt, und nicht um den Schwerpunkt!
T_b := Matrix(NL, 1):
T := 0:
for i to NL do
  I_i_i_tensor := Matrix([[I_i_i[1, i], I_i_i[2, i], I_i_i[3, i]], [I_i_i[2, i], I_i_i[4, i], I_i_i[5, i]], [I_i_i[3, i], I_i_i[5, i], I_i_i[6, i]]]):
  T_b_rot := (1/2)*Multiply(Transpose(omega_i_i(1 .. 3, i)), Multiply(I_i_i_tensor, omega_i_i(1 .. 3, i))):
  T_b_trans := (1/2)*M[i, 1]*Multiply(Transpose(rD_i_i(1 .. 3, i)), rD_i_i(1 .. 3, i))+Multiply(Transpose(rD_i_i(1 .. 3, i)), CrossProduct(omega_i_i(1 .. 3, i), mr_i_i_Si(1 .. 3, i))):
  T_b[i, 1] := T_b_rot+T_b_trans:
  printf("%s. Kinetische Energie für Körper %d berechnet (im Körper-KS, mit Parametersatz 2).\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), i-1):#0=Basis
  # Terme vereinfachen (nur einzelne Energien und nicht die Summe).
  # Annahme: Zusammenfassung verschiedener Körper nicht zielführend
  if use_simplify>=2 then
    tmp_t1:=time(): tmp_l1 := length(T_b[i,1]):
    printf("%s. Beginne Vereinfachung für kinetische Energie (Param. 2; Körper %d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l1):
    T_b[i,1] := simplify2(T_b[i,1]):
    tmp_t2:=time(): tmp_l2 := length(T_b[i,1]):
    printf("%s. Terme für kinetische Energie (Param. 2) vereinfacht (Körper %d). Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end if:
  T := T+T_b[i, 1]:

  # Terme vereinfachen (optional, Standardmäßig nur kinetische Energie einzelner Körper optimieren)
  # Für komplizierte Systeme hängt sich Maple hier auf.
  if use_simplify>=3 then
    tmp_t1:=time(): tmp_l1 := length(T):
    printf("%s. Beginne Vereinfachung für kinetische Energie (Param. 2; Summe bis Körper %d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l1):
    T := simplify2(T):
    tmp_t2:=time(): tmp_l2 := length(T):
    printf("%s. Terme für kinetische Energie (Param. 2) vereinfacht (Körper %d). Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), i-1, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end if:
end do:
# Maple Export
save T, sprintf("../codeexport/%s/tmp/energy_kinetic_floatb_%s_linkframe_par2_maple.m", robot_name, base_method_name):
# Potential Energy
# Berechnung in Welt-KS ist bereits erfolgreich. Wird hier nicht nochmal durchgeführt.
# siehe robot_tree_floatb_twist_rotmat_energy_worldframe_par2.mw
# Export
printf("%s: Kinetische Energie (Param. 2) berechnet und gespeichert. Beginne Matlab-Export.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S")):
# Matlab Export
# Floating Base
# Eliminiere die Basis-Position (nicht: Orientierung). Die kinetische Energie kann nicht davon abhängen.
# Falls die Ausdrücke doch vorkommen, liegt es daran, dass Maple diese nicht wegoptimieren konnte.
T_s:=convert_t_s(T):
for i from 1 to 3 do
  T_s := subs({X_base_s[i,1]=0},T_s):
end do:
if codegen_act then
  MatlabExport(T_s, sprintf("../codeexport/%s/tmp/energy_kinetic_floatb_%s_linkframe_par2_matlab.m", robot_name, base_method_name), codegen_opt):
end if:
# Fixed Base
T_s_fixb:=T_s:
for i from 1 to NQB do
  T_s_fixb := subs({X_base_s[i,1]=0},T_s_fixb):
end do:
for i from 1 to 6 do
  T_s_fixb := subs({V_base_s[i,1]=0},T_s_fixb):
end do:
if codegen_act then
  MatlabExport(T_s_fixb, sprintf("../codeexport/%s/tmp/energy_kinetic_fixb_linkframe_par2_matlab.m", robot_name), codegen_opt):
end if:


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: lda_exc *)

$define lda_c_pw_params
$define lda_c_pw_modified_params
$include "lda_c_pw.mpl"

alpha := (4/(9*Pi))^(1/3):

# Eq. (46) of Phys. Rev. A 73, 032506 (2006)
# I am not sure if this is the on-top pair correlation functional
# intended by the authors
g0_C    :=  0.08193:
g0_D    := -0.01277:
g0_E    :=  0.001859:
g0_dd   :=  0.7524:
g0_aHD  := -alpha*(Pi^2 + 6*log(2) - 3)/(5 * Pi):
g0_B    := -2*g0_aHD - g0_dd:
pmgb_g0 := rs ->
        (1 - g0_B*rs + g0_C*rs^2 + g0_D*rs^3 + g0_E*rs^4)*exp(-g0_dd*rs)/2:

pmgb_gc0 := rs -> pmgb_g0(rs) - 1/2:

# Eq. (14)
pmgb_phi := (n, z) ->
         (opz_pow_n(z,n/3) + opz_pow_n(-z,n/3))/2:

# Eq. (22)
pmgb_Q_a := 5.84605:
pmgb_Q_c := 3.91744:
pmgb_Q_d := 3.44851:
pmgb_Q_b := pmgb_Q_d - 3*Pi*alpha/(4*log(2) - 4):

pmgb_Q := x ->
       ((2*log(2) - 2)/Pi^2) * log(
        (1 + pmgb_Q_a*x + pmgb_Q_b*x^2 + pmgb_Q_c*x^3)/
        (1 + pmgb_Q_a*x + pmgb_Q_d*x^2)
       ):

# Eq. (31)
pmgb_b0 := (rs) -> 0.784949*rs:

# Eq. (32)
pmgb_gpp1 := (rs) -> (2^(5/3)/(5*alpha^2*rs^2)) *
          (1 - 0.02267*rs)/(1 + 0.4319*rs + 0.04*rs^2):

# Eq. (33)
pmgb_D2 := (rs) -> (-0.388*rs + 0.676*rs^2)*exp(-0.547*rs)/rs^2:

# Eq. (34)
pmgb_D3 := (rs) -> (-4.95*rs + rs^2)*exp(-0.31*rs)/rs^3:

# Eq. (28)
pmgb_cc4 := (rs, z) ->
       + (opz_pow_n( z,2)/4) * pmgb_gpp1(rs*(2/(1 + z))^(1/3))
       + (opz_pow_n(-z,2)/4) * pmgb_gpp1(rs*(2/(1 - z))^(1/3))
       + (1 - z^2) * pmgb_D2(rs)
       - pmgb_phi(8, z)/(5*alpha^2*rs^2):

# Eq. (29)
pmgb_cc5 := (rs, z) ->
       + (opz_pow_n( z,2)/4) * pmgb_gpp1(rs*(2/(1 + z))^(1/3))
       + (opz_pow_n(-z,2)/4) * pmgb_gpp1(rs*(2/(1 - z))^(1/3))
       + (1 - z^2) * pmgb_D3(rs):

# Eq. (30)
pmgb_C2 := (rs, z) ->
        -3*(1 - z^2)*pmgb_gc0(rs)/(8*rs^3):

pmgb_C3 := (rs, z) ->
        -(1 - z^2)*pmgb_g0(rs)/(sqrt(2*Pi)*rs^3):

pmgb_C4 := (rs, z) ->
        -9*pmgb_cc4(rs, z)/(64*rs^3):

pmgb_C5 := (rs, z) ->
        -9*pmgb_cc5(rs, z)/(40*sqrt(2*Pi)*rs^3):

pmgb_a1 := (rs, z) ->
        4*pmgb_b0(rs)^6*pmgb_C3(rs, z) + pmgb_b0(rs)^8*pmgb_C5(rs, z):

pmgb_a2 := (rs, z) ->
        4*pmgb_b0(rs)^6*pmgb_C2(rs, z) + pmgb_b0(rs)^8*pmgb_C4(rs, z)
        + 6*pmgb_b0(rs)^4*f_pw(rs, z):

pmgb_a3 := (rs, z) ->
        pmgb_b0(rs)^8*pmgb_C3(rs, z):

pmgb_a4 := (rs, z) ->
        pmgb_b0(rs)^8*pmgb_C2(rs, z) + 4*pmgb_b0(rs)^6*f_pw(rs, z):

pmgb_a5 := (rs, z) ->
        pmgb_b0(rs)^8*f_pw(rs, z):

# Eq. (26)
pmgb_ec_LR := (rs, z) -> (
           + pmgb_phi(2, z)^3*pmgb_Q(p_a_hyb_omega_0_*sqrt(rs)/pmgb_phi(2, z))
           + pmgb_a1(rs, z)*p_a_hyb_omega_0_^3
           + pmgb_a2(rs, z)*p_a_hyb_omega_0_^4
           + pmgb_a3(rs, z)*p_a_hyb_omega_0_^5
           + pmgb_a4(rs, z)*p_a_hyb_omega_0_^6
           + pmgb_a5(rs, z)*p_a_hyb_omega_0_^8
           )/(1 + pmgb_b0(rs)^2*p_a_hyb_omega_0_^2)^4:

f := (rs, z) -> pmgb_ec_LR(rs, z):


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)
(* prefix:
  gga_c_wi_params *params;

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

f_num := xt -> params_a_a + params_a_b*xt^2*exp(-params_a_k*xt^2):
f_den := (rs, xt) -> params_a_c + rs*(1 + params_a_d*(4*Pi/3)^(1/3)*xt^(7/2)):

f := (rs, zeta, xt, xs0, xs1) -> f_num(xt)/f_den(rs, xt):


with(DifferentialAlgebra):
ring_diff := DifferentialRing(blocks = [[N, S, M, P, E], [y1, y3, y2] ], derivations = [t]):
sys := [
diff(N(t), t) - (-delta_NE*N(t)*P(t) - mu_N*N(t)),
diff(S(t), t) - (-mu_LE*S(t)*E(t) + delta_EL*S(t) - S(t)^2*mu_LL - S(t)*delta_LM),
diff(M(t), t) - (S(t)*delta_LM - M(t)*mu_M),
diff(P(t), t) - (rho_P*P(t)^2 - S(t)*P(t)*mu_PL - E(t)*mu_PE*P(t) - P(t)*mu_P),
diff(E(t), t) - (-mu_EE*E(t)^2 + delta_NE*N(t)*P(t) - delta_EL*E(t) + E(t)*P(t)*rho_E),
y1(t) - (N(t)),
y3(t) - (M(t)),
y2(t) - (S(t) + E(t))
];
res := CodeTools[CPUTime](RosenfeldGroebner(sys, ring_diff, singsol=none));


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_exc *)

params_a_mu := MU_GE:
params_a_b  := 0.4:
params_a_c  := 2.14951:
params_a_e  := 1.987:

vt84_gamma := 0.000023:
tpss_ff    := z -> 3:
tpss_kappa := (x, t) ->
  1/(vt84_gamma/params_a_mu^2 + vt84_gamma/params_a_mu + 1):

$include "tpss_x.mpl"

(* Equation (8) *)

vt84_f   := (x, u, t) -> 1
    + tpss_fx(x, t)*exp(-vt84_gamma*tpss_fx(x, t)/params_a_mu)/(1 + tpss_fx(x, t))
    + (1 - exp(-vt84_gamma*tpss_fx(x, t)^2/params_a_mu^2))*(params_a_mu/tpss_fx(x, t) - 1):

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


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)
(* prefix:
  gga_x_pbeint_params *params;

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

pbeint_mu := s -> params_a_muGE + (params_a_muPBE - params_a_muGE)* \
   params_a_alpha*s^2/(1 + params_a_alpha * s^2):

(* this is the gga_x_pbe expression *)
pbeint_f0 := s -> 1 + params_a_kappa * (1 - params_a_kappa/(params_a_kappa + pbeint_mu(s)*s^2)):
pbeint_f  := x -> pbeint_f0(X2S * x):

f := (rs, z, xt, xs0, xs1) -> gga_exchange(pbeint_f, rs, z, xs0, xs1):


############################################################
# A function evaulator routine that
# finds the formation of trap surface
# i.e. blackhole formation, see the paper
# for its definition: http://arxiv.org/pdf/1508.01614v2.pdf
############################################################
read "/home/arman/FD/FD.mpl":

CFD();
MFD();

grid_functions:= {a1,b1,alpha,beta,ctfm,ctfmp};

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

spec := [
  { i = [1,1,1] , b=xmin } = 1+myzero*x(i),
  { i = [2,Nx-1,1]       } = (AVGT(Gen_Sten(1/alpha(t,x)))*Gen_Sten(diff(b1(t,x),t)) + AVGT( Gen_Sten( (1/a1(t,x) - beta(t,x)/alpha(t,x))*( diff(b1(t,x),x)/ctfmp(x) + b1(t,x)/ctfm(x) ) ) ) )/ (AVGT(Gen_Sten(b1(t,x)))),
  { i=[Nx,Nx,1], b=xmax  } = myzero*x(i)
];

A_Gen_Eval_Code(spec,input="d",proc_name="compute_th");


make_complex_projective_space := proc(n::nonnegint)
 local T,x,i;

 T := table([]):
 T["name"] := sprintf("CP^%d",n);
 T["dimension"] := 2*n;
 T["cohomology_gens"] := tdeg(x);
 T["cohomology_degrees"] := table([x = 2]);
 T["cohomology_rels"] := [x^(n+1)];
 T["cohomology_basis"] := [seq(x^i,i=0..n)];
 T["cohomology_graded_basis"] := table([]):
 for i from 0 to 2*n do
  T["cohomology_graded_basis"][i] :=
   `if`(modp(i,2)=0,[x^(i/2)],[]);
 od:

 return eval(T);
end:


restart;
with(LinearAlgebra):
with(VectorCalculus):
with(plottools):
BasisFormat(false):

MNAC := proc(w1,w2,x,t,epsilon)
	local z1, z2, L, i;
	z1 := w1.x;
	z2 := exp~(w2.log~(abs(z1) +~ epsilon));

	L := (z2 -~ t)^~2;
	return add(L[i],i=1..numelems(L));
end proc:

MNACalt := proc(w1,w2,x,t)
	local z1, z2, zhat2, w2ones, L, i;

	z1 := w1.x;
	zhat2 := log~(Transpose(w2) *~ z1 +~ 1 -~ w2);
	z2 := exp~(zhat2 . Matrix(ColumnDimension(w2),1,1));
	L := (z2 -~ t)^~2;
	return add(L[i],i=1..numelems(L));
end proc:

P := plot3d(
	MNAC(<<w[1]|w[1]|0|0>, <w[1]|w[1]|w[1]|w[1]>>, <<w[2]|w[2]>>, <1,1.2,1.8,2>, 13.2, 1E-7),
	w[1] = -1 .. 1, w[2] = -1 .. 1, view = [-1..1, -1..1, 0..500],
	axes=boxed, orientation=[-45, 39, 0]):
Export(FileTools:-JoinPath([currentdir(), "..", "paper", "graphics", "nac-mul-eps-1em7.jpeg"]), P);

P := plot3d(
	MNAC(<<w[1]|w[1]|0|0>, <w[1]|w[1]|w[1]|w[1]>>, <<w[2]|w[2]>>, <1,1.2,1.8,2>, 13.2, 1E-1),
	w[1] = -1 .. 1, w[2] = -1 .. 1, view = [-1..1, -1..1, 0..500],
	axes=boxed, orientation=[-45, 39, 0]):
Export(FileTools:-JoinPath([currentdir(), "..", "paper", "graphics", "nac-mul-eps-1em1.jpeg"]), P);

P := plot3d(
	MNAC(<<w[1]|w[1]|0|0>, <w[1]|w[1]|w[1]|w[1]>>, <<w[2]|w[2]>>, <1,1.2,1.8,2>, 13.2, 1),
	w[1] = -1 .. 1, w[2] = -1 .. 1, view = [-1..1, -1..1, 0..500],
	axes=boxed, orientation=[-45, 39, 0]):
Export(FileTools:-JoinPath([currentdir(), "..", "paper", "graphics", "nac-mul-eps-1.jpeg"]), P);

P := plot3d(
	MNACalt(<<w[1]|w[1]|0|0>, <w[1]|w[1]|w[1]|w[1]>>, <w[2]|w[2]>, <1,1.2,1.8,2>, 13.2),
	w[1] = -1 .. 1, w[2] = 0 .. 1, view = [-1..1, 0..1, 0..500],
	axes=boxed, orientation=[-45, 39, 0]):
Export(FileTools:-JoinPath([currentdir(), "..", "paper", "graphics", "nac-mul-nmu.jpeg"]), P);


INF = #7ff0000000000000
NAN = #7ff8000000000000
EPS = #3cb0000000000000

TRUE = 1
FALSE = 0

E = 2.71828182845904523536
PI = 3.141592653589793236462643
GAMMA = 0.577215664901632860606512 ~ Euler-Mascheroni constant
I = (0,1)

function string(x)
    return x*" "
end

function value(x)
    return x
end

REAL_TYPE = 1
COMPLEX_TYPE = 2
VECTOR_TYPE = 3
CVECTOR_TYPE = 4
MATRIX_TYPE = 5
CMATRIX_TYPE = 6
STRING_TYPE = 7
LIST_TYPE = 8

function isreal(x)
    return type(x) == REAL_TYPE
end

function iscomplex(x)
    return type(x) == COMPLEX_TYPE
end

function isvector(x)
    return type(x) == VECTOR_TYPE
end

function iscvector(x)
    return type(x) == CVECTOR_TYPE
end

function ismatrix(x)
    return type(x) == MATRIX_TYPE
end

function iscmatrix(x)
    return type(x) == CMATRIX_TYPE
end

function isstring(x)
    return type(x) == STRING_TYPE
end

function islist(x)
    return type(x) == LIST_TYPE
end

function rows(m)
    s = size(m)
    return s[0]
end

function cols(m)
    s = size(m)
    return s[1]
end

function left(s, n)
    if n <= size(s) then
        return s
    end
    return s + (n-size(s))*" "
end

function right(s, n)
    if n <= size(s) then
        return s
    end
    return (n-size(s))*" " + s
end

function center(s, n)
    if n <= size(s) then
        return s
    end
    d = n - size(s)
    l = round(d/2)
    r = d - l
    return l*" " + s + r*" "
end

function min(x, y)
    if x <= y then
        return x
    end
    return y
end

function max(x, y)
    if x >= y then
        return x
    end
    return y
end

function cis(x)
    return (cos(x), sin(x))
end

function pow(x, y)
    return x^y
end

function csc(x)
    return 1/sin(x)
end

function sec(x)
    return 1/cos(x)
end

function cot(x)
    return 1/tan(x)
end

function acsc(x)
    return asin(1/x)
end

function asec(x)
    return acos(1/x)
end

function acot(x)
    return atan(1/x)
end

function csch(x)
    return 1/sinh(x)
end

function sech(x)
    return 1/cosh(x)
end

function coth(x)
    return 1/tan(x)
end

function acsch(x)
    return asinh(1/x)
end

function asech(x)
    return acosh(1/x)
end

function acoth(x)
    return atanh(1/x)
end

function mag(x)
    m = 0
    for i in 0:length(x)
        m = m + x[i]*x[i]
    end
    return sqrt(m)
end

function cross(x, y)
    return [x[1]*y[2] - x[2]*y[1], x[2]*y[0] - x[0]*y[2], x[0]*y[1] - x[1]*y[0]]
end

function islower(s)
    c = s[0]
    if "a" <= c and c <= "z" then
        return TRUE
    end
    return FALSE
end

function isupper(s)
    c = s[0]
    if "A" <= c and c <= "Z" then
        return TRUE
    end
    return FALSE
end

function tolower(s)
    rv = ""
    for i in 0:size(s)-1
        if isupper(s[i]) then
	    rv = rv + char(ascii(s[i]) + 32)
	else
	    rv = rv + s[i]
	end
    end
    return rv
end

function toupper(s)
    rv = ""
    for i in 0:size(s)-1
        if islower(s[i]) then
	    rv = rv + char(ascii(s[i]) - 32)
	else
	    rv = rv + s[i]
	end
    end
    return rv
end


# Acceleration Calculation for the Robot based on MDH frames
# Introduction
# Berechnung der Beschleunigung von Koordinatensystemen und Schwerpunkten
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# tree -> Berechnung für eine beliebige Baumstruktur (ohne Schleifen)
# floatb_twist -> floating base wird durch base twist (Geschwindigkeit der Basis) berücksichtigt
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# acceleration -> Berechnung der Beschleunigung aller Segmente
# linkframe -> Berechnung der Geschwindigkeit im Körper-KS (KSi)

# Sources
# [KhalilDombre2002] Modeling, Identification and Control of Robots
# [Ortmaier2014] Vorlesungsskript Robotik I
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(ArrayTools):
with(codegen):
with(CodeGeneration):
with(StringTools):
codegen_act := true:
codegen_opt := 2:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotx":
read "../transformation/proc_troty":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../transformation/proc_trafo_mdh":
read "../robot_codegen_definitions/robot_env":
printf("Generiere Beschleunigung für %s (Herleitung im Körper-KS)\n", robot_name):
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
read sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
kin_constraints_exist := kin_constraints_exist: # nur zum Abschätzen der Komplexität
;
# Term-Vereinfachungen einstellen
if not assigned(simplify_options) or simplify_options(5)=-1 then # Standard-Einstellungen:
  if not kin_constraints_exist then # normale serielle Ketten und Baumstrukturen
    use_simplify := 0: # Standardmäßig aus
  else # mit kinematischen Zwangsbedingungen
    if NJ <= 5 then # bei einfachen Systemen vereinfachungen vornehmen
      use_simplify := 1
    else # bei längeren kinematischen Ketten dauert es zu lange / bringt zu wenig
      use_simplify := 0:
    end if:
  end if:
else # Benutzer-Einstellungen:
  use_simplify := simplify_options(5): # fünfter Eintrag ist für Beschleunigung
end if:
# Ergebnisse der Kinematik laden
read sprintf("../codeexport/%s/tmp/kinematics_floatb_%s_rotmat_maple.m", robot_name, base_method_name):
Trf := Trf:
Trf_c := Trf_c:
# Ergebnisse der Geschwindigkeit laden
read sprintf("../codeexport/%s/tmp/velocity_linkframe_floatb_%s_maple.m", robot_name, base_method_name):
omega_i_i:=omega_i_i:
rD_i_i:=rD_i_i:
# Zeitableitungen der MDH-Drehwinkel laden.
# Die Berechnung soll nur an einer Stelle erfolgen. Siehe robot_tree_velocity_mdh_angles.mw.
read sprintf("../codeexport/%s/tmp/velocity_mdh_angles_maple.m", robot_name):
thetaD := thetaD:
dD :=dD:
# Zeitableitungen der Geschwindigkeit laden.
# Die Berechnung soll nur an einer Stelle erfolgen. Siehe robot_tree_acceleration_mdh_angles.mw.
read sprintf("../codeexport/%s/tmp/acceleration_mdh_angles_maple.m", robot_name):
thetaDD :=thetaDD:
dDD := dDD:

# Calculate Acceleration
# First assume fixed base model with base velocity and acceleration set to zero
# Anfangsgeschwindigkeit definieren für floating base model
# Acceleration of Frames
#
# Beschleunigung der Basis der Koordinatensysteme der Körper (ausgedrückt im Körper-KS)
rDD_i_i := Matrix(3, NL):
# Winkelbeschleunigung von Körper i ausgedrückt im Körper-KS
omegaD_i_i := Matrix(3, NL):
# Anfangsgeschwindigkeit der Basis:
# twist: Basis-Geschwindigkeit bzgl Welt-KS ausgedrückt im Basis-KS
# eulangrpy: V_base_t beinhaltet die Geschwindigkeit der Basis im Welt-KS, ausgedrückt im Welt-KS. Daher ist für eine Darstellung im Körper-KS noch die Rotation erforderlich.
if base_method_name = "twist" then:
  rDD_i_i(1..3,1) := diff~(V_base_t(1..3,1),t):
  omegaD_i_i(1..3,1) := diff~(V_base_t(4..6,1),t):
end:
if base_method_name = "eulxyz" then:
  rDD_i_i(1..3,1) := Transpose(Trf_c(1..3, 1..3, 1)) . diff~(V_base_t(1..3,1),t):
  omegaD_i_i(1..3,1) := Transpose(Trf_c(1..3, 1..3, 1)) . T_basevel . diff~(V_base_t(4..6,1),t):
end:
# Erhöhe die Beschleunigung jedes Körpers
# Betrachte dazu nur die durch Gelenke angetriebenen Körper, nicht die Basis
for i from 1 to NJ do # Gelenke durchgehen
  # Körper der von Gelenkwinkel i bewegt wird: Körperindex i+1
  # Vorgängerkörper bestimmen
  j := v(i) + 1:
  # Geschwindigkeit des Vorgängers; Trf_c(...,i+1) enthält z-Achse des Körperkoordinatensystems, das von qi bewegt wird.
  # [Ortmaier2014] (7.7) (S.115) (dort falsche Indizes für MDH), [KhalilDombre2002] (9.14)
  R_j_i := Trf(1..3,1..3,i): # Rotation vom Vorgänger-Körper (j) zu diesem Körper (i+1)
  R_i_j := Transpose(R_j_i):
  # [GautierKhalil1988], equ.7: omega_jj aus [GautierKhalil1988] entspricht omega_i_i(1 .. 3, i+1) hier
  # [Ortmaier](7.14) (S.99) & (7.19) (S.99)
  if sigma(i) = 0 then # Drehgelenk
    omegaD_i_i(1 .. 3, i+1) := Matrix(Multiply(R_i_j,( Matrix(3,1,omegaD_i_i(1 .. 3, j))))) + Matrix( thetaDD(i,1)*<0;0;1>) + Matrix(CrossProduct(R_i_j.Matrix(3,1,omega_i_i(1 .. 3, j)),thetaD(i,1)*<0;0;1>)) :
  else: # Schubgelenk
    omegaD_i_i(1 .. 3, i+1) := Multiply(R_i_j, Matrix(3,1,omegaD_i_i(1 .. 3, j))):
  end if:
  # Terme vereinfachen (Teil 1)
  if use_simplify>=1 then
    tmp_t11:=time():
    tmp_l11 := length(omegaD_i_i(1 .. 3, i+1)):
    omegaD_i_i(1 .. 3, i+1) := simplify2(omegaD_i_i(1 .. 3, i+1)):
    tmp_l21 := length(omegaD_i_i(1 .. 3, i+1)):
    tmp_t21:=time():
  end if:
  # Vektor vom Ursprung des vorherigen Koordinatensystems zu diesem KS
  r_j_j_i := Trf(1 .. 3, 4, i):
  # [GautierKhalil1988], equ.8: v_jj aus [GautierKhalil1988] entspricht rD_i_i(1 .. 3, i+1) hier
  # [Ortmaier](7.17) (S.99) & (7.22) (S.100)
  if sigma(i) = 0 then # Drehgelenk
    rDD_i_i(1 .. 3, i+1) := Multiply( R_i_j, ( rDD_i_i(1 .. 3, j) + omegaD_i_i(1 .. 3, j) &x r_j_j_i + CrossProduct(omega_i_i(1..3,j),(omega_i_i(1..3,j) &x r_j_j_i )))) :
  else: # Schubgelenk
    rDD_i_i(1 .. 3, i+1) := Matrix( Multiply( R_i_j, ( rDD_i_i(1 .. 3, j)  + CrossProduct(omega_i_i(1..3,j), (omega_i_i(1..3,j) &x r_j_j_i )) + CrossProduct(omegaD_i_i(1..3,j), r_j_j_i)))) + Matrix( 2* dD(i,1) * CrossProduct(R_i_j.omega_i_i(1..3,j),<0;0;1>)) + Matrix(dDD(i,1)*<0;0;1>)  :
  end if:
  printf("Beschleunigung für Körperkoordinatensystem %d aufgestellt (Herleitung im Körper-KS). %s\n", i, FormatTime("%Y-%m-%d %H:%M:%S")): #0=Basis
  # Terme vereinfachen (Teil 2)
  if use_simplify>=1 then
    tmp_t12:=time():
    tmp_l12 := length(rDD_i_i(1 .. 3, i+1)):
    rDD_i_i(1 .. 3, i+1) := simplify2(rDD_i_i(1 .. 3, i+1)):
    tmp_l22 := length(rDD_i_i(1 .. 3, i+1)):
    tmp_t22:=time():
    printf("%s: Terme für Beschleunigungen vereinfacht. Länge: %d->%d / %d->%d. Rechenzeit %1.1fs und %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), tmp_l11, tmp_l21, tmp_l12, tmp_l22, tmp_t21-tmp_t11, tmp_t22-tmp_t12):
  end if:
end do:

# Acceleration of Center of Mass

rDD_i_Si := Matrix(3, NL):

for i to NL do
  rDD_i_Si(1 .. 3, i) := Matrix(rDD_i_i(1 .. 3, i))+Matrix(CrossProduct(omegaD_i_i(1 .. 3, i), r_i_i_Si(1 .. 3, i)))+Matrix(CrossProduct(omega_i_i(1 .. 3, i), CrossProduct(omega_i_i(1 .. 3, i), r_i_i_Si(1 .. 3, i)))):
  printf("Beschleunigung für Körperschwerpunkt %d aufgestellt. %s\n", i-1, FormatTime("%Y-%m-%d %H:%M:%S")):
  # Terme vereinfachen
  if use_simplify>=1 then
    tmp_t1:=time(): tmp_l1 := length(rDD_i_Si(1 .. 3, i)):
    rDD_i_Si(1 .. 3, i) := simplify2(rDD_i_Si(1 .. 3, i)):
    tmp_t2:=time(): tmp_l2 := length(rDD_i_Si(1 .. 3, i)):
    printf("%s: Terme für Schwerpunkts-Beschleunigungen vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end if:
end do:
# Export
# Maple Export
save omegaD_i_i, rDD_i_i,rDD_i_Si , sprintf("../codeexport/%s/tmp/acceleration_linkframe_floatb_%s_maple.m", robot_name, base_method_name):
printf("Maple-Ausdrücke exportiert. %s\n", FormatTime("%Y-%m-%d %H:%M:%S")):

# Matlab Export

if codegen_act then
  MatlabExport(convert_t_s(omegaD_i_i), sprintf("../codeexport/%s/tmp/acceleration_omegaDii_floatb_%s_linkframe_matlab.m", robot_name, base_method_name), codegen_opt):
  MatlabExport(convert_t_s(rDD_i_i), sprintf("../codeexport/%s/tmp/acceleration_rDDii_floatb_%s_linkframe_matlab.m", robot_name, base_method_name), codegen_opt):
  MatlabExport(convert_t_s(rDD_i_Si), sprintf("../codeexport/%s/tmp/acceleration_rDDiSi_floatb_%s_linkframe_matlab.m", robot_name, base_method_name), codegen_opt):
  printf("Beschleunigung in Matlab exportiert. %s\n", FormatTime("%Y-%m-%d %H:%M:%S")):
end if:


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_exc *)

$define gga_c_pbe_params
$include "gga_c_pbe.mpl"

pkzb_c := 0.53:

pkzb_perp := (rs, z, xt, xs0, xs1, ts0, ts1) ->
  + (1 + pkzb_c*(t_total(z, xs0^2, xs1^2)/(8*t_total(z, ts0, ts1)))^2)
  * f_pbe(rs, z, xt, xs0, xs1):

pkzb_par  := (rs, z, xt, xs0, xs1, ts0, ts1) ->
  - (1 + pkzb_c)*(
    + (xs0^2/(8*ts0))^2*gga_stoll_par(f_pbe, rs,  z, xs0,  1)
    + (xs1^2/(8*ts1))^2*gga_stoll_par(f_pbe, rs, -z, xs1, -1)
):

pkzb_f := (rs, z, xt, xs0, xs1, ts0, ts1) ->
  pkzb_perp(rs, z, xt, xs0, xs1, ts0, ts1) + pkzb_par(rs, z, xt, xs0, xs1, ts0, ts1):

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


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

k  := 6.5124: (* PBEsol transformation *)
xi := p -> 2*p/(k + p) - 1:
xj := a -> - (1 - a^2)^3/(1 + a^3*(1 + a^3)):

with(orthopoly):

mgga_mbeef := (x, t) -> add(add(
  + coefs_mbeef[i][j]
  * P(j-1, xi(X2S^2*x^2))
  * P(i-1, xj((t - x^2/8)/K_FACTOR_C)),
i=1..n_mbeef), j=1..n_mbeef):


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)
(* prefix:
  gga_x_sogga11_params *params;

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

sogga11_alpha := params_a_mu*X2S*X2S/params_a_kappa:

sogga11_f0 := x -> 1 - 1/(1 + sogga11_alpha*x^2):
sogga11_f1 := x -> 1 - exp(-sogga11_alpha*x^2):

sogga11_f  := x -> add(params_a_a[i]*sogga11_f0(x)^(i-1), i=1..6) + add(params_a_b[i]*sogga11_f1(x)^(i-1), i=1..6):

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


TITLE
   Farmer;

STOCHASTIC

SCENARIO
   sc := 1..3;

INDEX
   crop    := (Wheat,Corn,SugarB);
   prLevel := 1..2;

PROBABILITIES
   Prob[sc] := (1/3, 1/3, 1/3);

RANDOM DATA
   Yield[crop,sc] := SPARSEFILE("Farmer.dat");

DATA
   Cost[crop] := (150,230,260);
   SellPrice[crop,prLevel]   := (170,  0,
                                 150,  0,
                                  36, 10);
   PurPrice[crop] := (238, 210, 0);
   MinReq[crop] := (200,240,  0);
   AvailLand := 500;
   MaxBeets := 6000;

STAGE1 VARIABLES
   Plant[crop] -> Pl;

STAGE2 VARIABLES
   CropPurchased[crop] WHERE (PurPrice > 0) -> Cp;
   CropSold[crop,prLevel] WHERE (SellPrice > 0) -> Cs;

MODEL
   MAX Profit =  SUM(crop,prLevel: SellPrice * CropSold)
              - SUM(crop: PurPrice * CropPurchased)
              - SUM(crop: Cost * Plant);

SUBJECT TO

   LandUse: SUM(crop: Plant) <= AvailLand;

   MeetMinReq[crop] -> MR:   Yield * Plant
                    + CropPurchased
                    - SUM(prLevel: CropSold)
                   >= MinReq;

BOUNDS
   CropSold[crop:=SugarB,prLevel:=1] <= MaxBeets;

END


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: gga_exc *)
(* prefix:
  gga_x_optx_params *params;

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

optx_f := x-> params_a_a + params_a_b*(params_a_gamma*x^2/(1 + params_a_gamma*x^2))^2:

f := (rs, z, xt, xs0, xs1) -> gga_exchange(optx_f, rs, z, xs0, xs1):


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_exc *)

b97mv_par_n := 6:

b97mv_gamma_x := 0.004:
b97mv_par_x := [
    [  0.85,  0, 0],
    [  1.007, 0, 1],
    [  0.259, 1, 0],
    [  0,   0, 0],
    [  0,   0, 0],
    [  0,   0, 0]
]:

b97mv_gamma_ss := 0.2:
b97mv_par_ss := [
    [  0.443,  0, 0],
    [ -1.437,  0, 4],
    [ -4.535,  1, 0],
    [ -3.39,   2, 0],
    [  4.278,  4, 3],
    [  0,    0, 0]
]:

b97mv_gamma_os := 0.006:
b97mv_par_os := [
    [  1.000,  0, 0],
    [  1.358,  1, 0],
    [  2.924,  2, 0],
    [ -8.812,  2, 1],
    [ -1.39,   6, 0],
    [  9.142,  6, 1]
]:

$include "lda_x_erf.mpl"
$include "b97mv.mpl"

wb97mv_f := (rs, z, xs0, xs1, ts0, ts1) ->
   my_piecewise3(screen_dens_zeta(rs,  z), 0, (1 + z)/2 * lda_x_erf_spin(rs*(2/(1 + z))^(1/3),  1)
    * b97mv_g(b97mv_gamma_x,  b97mv_wx_ss, b97mv_par_x,  b97mv_par_n, xs0, ts0, 0))
+  my_piecewise3(screen_dens_zeta(rs, -z), 0, (1 - z)/2 * lda_x_erf_spin(rs*(2/(1 - z))^(1/3),  1)
    * b97mv_g(b97mv_gamma_x,  b97mv_wx_ss, b97mv_par_x,  b97mv_par_n, xs1, ts1, 0)):

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


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: work_gga_x *)

a1 :=   0.041106:
a2 :=   2.626712:
a3 :=   0.092070:
a4 :=   0.657946:

f0 := s-> a1 * s^a2/(1 + a3 * s^a2)^a4:
f  := x-> f0(X2S*x):


(*
 Copyright (C) 2017 M.A.L. Marques

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_vxc *)

(* prefix:
  mgga_x_tb09_params *params;

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

(* replace: "br89_x\(" -> "xc_mgga_x_br89_get_x(" *)

$include "mgga_x_br89.mpl"

params_a_gamma := 0.8:
tb09_c_HEG := (3*params_a_c - 2)*sqrt(5/12)/(Pi):

# we add here a threshold of 1e-10 for either tau or the Fermi curvature
tb09_f := (rs, z, x, u, t) -> (params_a_c*X_FACTOR_C*br89_v(br89_x(br89_cQ(br89_Q(x, u, t))))
  + tb09_c_HEG*sqrt(2*m_max(t - params_a_alpha*x^2/8, 1e-10)))*n_spin(rs, z)^(1/3):

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