# # Weyl group related operations on weyl word
# #       by Shizuo Kaji
# requires coxeter and weyl package by J.Stembridge
# http://www.math.lsa.umich.edu/~jrs/maple.html
#
# global variables:
# R: Lie type
# B: the set of simple roots

WeylOps := module() option package;
  export `.`,inv,descent,minrep,PReducedWd,WeylFunc,e,c,symmetric,e2w,e2a,a2e,a2w,w2a,deg_X,dimr,relative_roots,e2ref,action,oneline;
  # inverse of weyl word
  inv := proc (weyl::list);
	return ListTools[Reverse](weyl);
  end proc;

  # operator '.' as product
  `.` := overload(
  [
    proc(a::list, b::list) option overload(callseq_only); global R;
       return(reduce([op(a),op(b)],R));
    end,
    proc(a::indexed, b::indexed) option overload(callseq_only);
        if op(0,a)=`s` and op(0,b)=`s` then
            return(s[op(1,a).op(1,b)]);
        else
            return(a*b);
        end if;
    end,
    proc(a::`+`, b::anything)  option overload(callseq_only);
        return map(`.`,a,b);
    end,
    proc(a::anything, b::`+`)  option overload(callseq_only);
        return map2(`.`,a,b);
    end,
    proc(a::`*`, b::anything)  option overload(callseq_only);
        return op(1,a)*((a/op(1,a)).b);
    end,
    proc(a::anything, b::`*`)  option overload(callseq_only);
        return op(1,b)*(a.(b/op(1,b)));
    end
  ]
):

# descent set of the weyl word "weyl"
descent := proc(weyl::list) local i, V; global R;
	V := {};
	for i from 1 to rank(R) do
	   if nops(weyl.[i]) < nops(weyl) then
    		V := {op(V), i};
	   end if;
    end do:
	return V;
end proc;

# minimal coset representative of "weyl" according to "roots"
minrep := proc(weyl::list, roots::set) local i,temp; global R;
  for i in roots do
      temp:=weyl.[i]:
      if nops(temp) < nops(weyl) then return(procname(temp, roots)); end if:
  end do:
  return(weyl):
end proc:

# enumerate all reduced words (minimal coset representatives) of "R" upto length "len"
PReducedWd := proc (R, roots::set:={}, maxlen::integer:=nops(longest_elt(R)))
  local B, r, v, i, j, len, newvects, newwords, words, oldvects;
    B := base(R); r := rank(B);
	newvects := [add(weights(R)[i], i={$1..r} minus roots)];
    words := Array(0..maxlen); words[0]:=[[]];
    for len from 1 to maxlen do;
        oldvects := newvects;
        newvects := []; newwords := NULL;
        for j to nops(oldvects) do
            for i to r do
                if 0 < iprod(B[i], oldvects[j]) then
                    v := reflect(B[i], oldvects[j]);
                    if not member(v, newvects) then
                        newvects := [op(newvects), v];
                        newwords := newwords, reduce([i,op(words[len-1][j])],R);
                    end if
                end if
            end do
        end do;
        if newwords=NULL then break; end if;
        words[len] := [newwords];
    end do;
    return convert(words[1..len-1],list);
end proc;

# iterative application of a function of simple roots for a weyl word poly
WeylFunc := proc(weyl, f::polynom, func::procedure) local i,temp;
	temp := f;
    if type(weyl,'numeric') then
        return weyl*f;
    elif type(weyl,'list') then
        for i from nops(weyl) to 1 by -1 do
            temp := func(weyl[i], temp);
        end do;
        return temp;
    elif type(weyl, 'indexed') then
        if op(0,weyl)=`s` then
            return procname(op(1,weyl), f, func);
        else
            return weyl*f;
        end if;
    elif type(weyl, linear) then
	  return add(coeff(weyl,ind)*procname(ind, f, func), ind=indets(weyl));
    else
        print("error in WeylFunc:",weyl);
    end if;
end proc:

## Utility Functions
# coordinate variables
e := proc (j::integer);
    return cat('e', j);
end proc;

# elementary symmetric function "c(i)" of "t[1..r]"
c := proc (deg::integer,sym::symbol := `t`) global R;
    return symmetric(deg, [seq(sym[i], i = 1 .. rank(R))])
end proc;

# elementary symmetric function of degree "deg" in "vars"
symmetric := proc (deg::integer, vars::list) local T;
    return sort(expand(coeff(mul(T+i, i=vars), T, nops(vars)-deg)));
end proc;

# convert vector in "e" into weights "w[1..r]"
e2w := proc (el::linear) global R;
    return add(weight_coords(el, R)[i]*w[i], i=1..rank(R));
end proc;

# convert vector in "e" into simple roots "a[1..r]"
e2a := proc (el::{polynom, list}) global R;
    if type(el, rational) then
	  return el;
    elif type(el, linear) then
      return add(root_coords(el, R)[i]*a[i], i=1..rank(R));
    elif type(el, `*`) or type(el, list) then
	  return map(procname, el);
    elif type(el, `^`) then
	  return procname(op(1,el))^op(2,el);
    else
        print("error in e2a:",el);
        return infinity;
    end if;
end proc;

# convert vector in "e" into simple roots "a[1..r]"
a2e := proc (apol::{polynom, list}) global B;
    return eval(apol,[seq(a[i]=B[i],i=1..nops(B))]);
end proc:

# convert a-poly into w-poly
a2w := proc (apol::polynom) global R,B;
    return eval(apol, [seq(a[j]=add(weight_coords(B[j], R)[i]*w[i], i=1..nops(B)), j=1..nops(B))]);
end proc;

# convert w-poly into a-poly
w2a := proc (wpol::polynom) global R,B;
    return eval(wpol, [seq(w[j]=add(root_coords(weights(R)[j], R)[i]*a[i], i=1..nops(B)), j=1..nops(B))]);
end proc;

# degree of a polynomial in X[]
deg_X := proc(f::polynom) local d,max_d,term,temp,x,y;
    if type(f,`+`) then
        max_d:=0;
        for term in f do
            d := procname(term);
            if d>max_d then max_d:=d; end if;
        end do;
        return max_d;
    elif type(f,`^`) then
        procname(op(1,f))*op(2,f);
    else
        temp := f;
        for x in indets(f) do
            temp := eval(temp, x=y^(nops(op(1,x))));
        end do;
        return degree(temp);
    end if;
end proc:

# dimension of the standard representation of R
dimr := proc(R) local lie_type; option remember;
	lie_type := name_of(R);
    if lie_type = cat('A',rank(R)) then
        return rank(R)+1;
    elif lie_type = G2 then
        return 3;
    elif lie_type = E6 or lie_type = E7 then
        return 8;
    else
        return rank(R);
    end if;
end proc:

# positive roots relative to a parabolic subgroup
relative_roots := proc(R,roots::set) local L;
    L:={seq(base(R)[i],i=roots)};
    L:=orbit(L,R);
    return {op(pos_roots(R))} minus {op(L)};
end proc:

# vector to reflection
e2ref:=proc(el) local i,w; global R;
   vec2fc(reflect(el,interior_pt(R)),R,'w');
   return w;
end proc:

# action of a reduced word on a linear sum of ei
action:=proc(w::list,el::polynom) global B;
    return reflect(seq(B[i],i=w),el);
end proc:

# one line notation of elements in classical groups
oneline:=proc(weyl::list) local deg, lie_type, L, i; global R,pg;
	lie_type := name_of(R);
    if lie_type = cat('A',rank(R)) then
        deg := rank(R)+1;
    elif lie_type = cat('B',rank(R)) or lie_type = cat('C',rank(R)) or lie_type = cat('D',rank(R)) then
        deg := 2*rank(R);
    else
        return -infinity;
    end if;
    L:=convert(multperm(weyl,pg), 'permlist', deg);
    if lie_type = cat('B',rank(R)) or lie_type = cat('C',rank(R)) or lie_type = cat('D',rank(R)) then
        L:=L[-rank(R)..-1];
        for i from 1 to nops(L) do;
            if L[i]<=rank(R) then
                L[i]:=L[i]-rank(R)-1;
            else
                L[i]:=L[i]-rank(R);
            end if;
        end do;
    end if;
    return convert(L,array);
end proc:

end module:


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

a1 := 3*Pi/X_FACTOR_C:

f := (rs, x, t, u) -> a1/(2*t - u/4):


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

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

RigidMotionsParameterSpaceDecompostion := module()
  option package;
  uses   RigidMotionsParameterSpaceDecompostionRecursive, RigidMotionsParameterSpaceCommon;

  local  GetQuadric, IsMonotonic, ComputeSetOfQuadrics,
         ComputeEventsATypeGrid, ComputeEventsBTypeGrid, ComputeEventsCTypeGrid,
         ComputeAsymptoticABEventsGrid, ComputeAsymptoticAAEvents,
         ComputeEventsFromAlgebraicNumbers, IsAsymptotic;

         # Only the procedures with prefix Launch* should be called! Other are exported only for
         # the grid framework.
  export IsAsymptoticIntersection, ComputeSamplePoints, ParallelComputeSamplePoints,
         LaunchComputeEvents, LaunchComputeSamplePoints;

  #Variables shared by grid nodes;
  global Q, events, vars, dbPath, skipped;

# Procedure: GetQuadric
#   Compute a quadric.
#
# Parameters:
#   R          - a 3x3 rotation matrix obtained from Cayley transform
#   neighbor   - a vector which points on a neighbor
#   hGridPlane - a half-grid plane i.e. k_dim + 1/2
#   axis       - a given row of R matrix
#
# Output:
#   Multivariate polynomial of degree 2 related to the changed of configuration of image patch and
#   given in [Quadrics:2016] as equation (7).
GetQuadric := proc(R::~Matrix, neighbor::~Vector, hGridPlane::~Vector, axis::integer)
  local r := R . neighbor - hGridPlane;
  if axis > 0 and axis < 4 then
    return simplify( ( 2 * denom( R[1][1] ) ) * r[axis] );
  else
    error "Wrong dimension: dim must be in [1,3].";
  end if:
end proc:

# Procedure: IsMonotonic
#   Check if given polynomial of degree 2 is non-positive or non-negative
#
# Parameters:
#   x      - a polynomial
#
# Output:
#   true if polynomial of degree 2 is non-positive and false otherwise
IsMonotonic := proc( x::~polynom )
  local homo, hessian, signmap, clean:
  homo := Groebner:-Homogenize( x, v ):
  hessian := 1 / 2 * VectorCalculus:-Hessian( homo, [ op( indets( x ) ), v ] ):
  signmap := map( signum, LinearAlgebra:-Eigenvalues( hessian ) ):
  signmap := convert( signmap, list ):
  clean := remove( `=`, signmap , 0 ):
  return ( andmap( `=`, clean, 1 ) or andmap( `=`, clean, -1 ) ):
end proc:

# Procedure: ComputeSetOfQuadrics
#   Compute a set of quadrics which are reduced by duplicated and these ones
#   which are strictly positive or negative.
#
# Parameters:
#   R          - a rotation matrix obtained from Cayley transform
#   nType      - size of neighborhood i.e. N_1, N_2, N_3.
#   axis       - a given axis i.e. 1 = x, 2 = y and 3 = z
#   kRange     - a range of planes to consider
#
# Output:
#   List of quadrics of form f - g reduced by duplicated---up to constant
#   factor---and these ones which are strictly positive or negative.
ComputeSetOfQuadrics := proc( R::~Matrix,
                               nType::string,
                               axis::~integer,
                               kRange::~list )
  local neighborhood := GetNeighborhood( nType ):
  local quadrics := Array([]):
  local f::polynom, g::polynom, quadric::polynom:
  local indexes := []:
  local T := combinat:-cartprod( [ neighborhood, neighborhood, kRange, kRange ] ):

  if LinearAlgebra:-Determinant(R) <> 1 or [upperbound(R)] <> [3,3] or nops(indets(R)) = 0 then
    error "Used matrix is not a correct 3 x 3 rotation matrix!";
  fi;

  if axis < 1 or axis > 3 then
    error "Axis is not from range [1,3].";
  fi:

  while not T[ finished ] do
    indexes := T[ nextvalue ]():
    f := GetQuadric( R, Vector( indexes[1] ), Vector( 3, indexes[3] + 1/2 ), axis ):
    g := GetQuadric( R, Vector( indexes[2] ), Vector( 3, indexes[4] + 1/2 ), axis ):
    quadric := f - g:
    if not type(quadric, constant) then
      quadric := quadric / lcoeff( quadric ):
      ArrayTools:-Append(quadrics,quadric):
    end if:
  end do:
  quadrics := convert(quadrics,set):
  quadrics := remove( IsMonotonic, quadrics ):
  quadrics := Threads:-Map(proc(x) normal(x) * denom(x) end proc, quadrics):
  return quadrics:
end proc:

# Procedure: IsAsymptotic
#   Checks if given quadric has an asymptotic critical value. For this moment a direction is fixed
#   to a.
#
# Parameters:
#   x          - a quadric in three variables
#   vars       - a list of variables
#
# Output:
#   List of solution for which partial derivatives in vars[2] and vars[3] are collinear.
IsAsymptotic := proc(x::polynom, vars::list)
  local vec, Vb, Vc, VV, sols;
  if nops(vars) < 3 then
    error "Expected at least three variables!";
  fi;
  vec := VectorCalculus:-Gradient(x, vars);
  Vb := Vector(3, [coeff(vec[2],vars[2]),coeff(vec[2],vars[3]),eval(vec[2],[vars[-2]=0,vars[-1]=0])]);
  Vc := Vector(3, [coeff(vec[3],vars[2]),coeff(vec[3],vars[3]),eval(vec[3],[vars[-2]=0,vars[-1]=0])]);
  VV := LinearAlgebra:-CrossProduct(Vb, Vc);
  if norm(VV,1) = 0 then
    return {{vars[1]=0}};
  fi;
  sols := solve({VV[1] = 0, VV[2] = 0,VV[3] = 0,vars[1]>=0}, [vars[1]]);
  if sols = NULL then
    return {};
  else
    return sols;
  fi;
end proc:

# Procedure: IsAsymptoticIntersection
#   Checks if intersection of two quadrics has an asymptotic critical value. For this moment a
#   direction is fixed to a.
#
# Parameters:
#   p          - a quadric in three variables
#   q          - a quadric in three variables
#   vars       - a list of variables
#
# Output:
#   List of solution for which intersection of p and q has an asymptotic intersection
# Comment:
#   - Since Groebner package seems to have memory leak I should rather replace
#     PolynomialIdeals:-EliminationIdeal by resultant elimination similarly to what I did with
#     univariate polynomials.
# TODO:
#   - Allow user to chose a direction.
#   - if there is no intersection between quadrics then skip it.
IsAsymptoticIntersection := proc( p::polynom, q::polynom, vars::list )
  local J := PolynomialIdeals:-`<,>`(p,q);
  local Pb, Pc, Cb, Cc, sols, univ;
  if nops(vars) < 3 then
    error "Expected at least three variables!";
  fi;
  Pb := PolynomialIdeals:-EliminationIdeal(J,{op(vars[1..2])}):
  Pb := PolynomialIdeals:-IdealInfo:-Generators(Pb)[1];
  Pc := PolynomialIdeals:-EliminationIdeal(J,{op(vars[[1,3]])});
  Pc := PolynomialIdeals:-IdealInfo:-Generators(Pc)[1];
  Cb := lcoeff(Pb, vars[2]);
  Cc := lcoeff(Pc, vars[3]);
  return lcm(Cb,Cc);
end proc:

# Procedure: ComputeEventsATypeGrid
#   Compute events such that a sweep plane is tangent to a quadric.
#
# Parameters:
#   Q          - a set of quadrics or conics
#   dim        - a list of indexes of variables used to calculate partial derivatives
#   vars       - a list of variables
#
# Output:
#   List of ranges which contains roots of a system(q, d/db q, d/dc q).
#
# Comment:
#  - only the first direction is supported
ComputeEventsATypeGrid := proc( Q, dim::list, vars::list )
  local s, result := Array([]);
  if nops(vars) < 3 then
    error "Expected at least three variables!";
  fi;
  s := proc(i::integer, vars::list)
   local sys, univ;
   local q := Q[i];
   sys := [q, diff( q, vars[ dim[1] ] ), diff( q, vars[ dim[2] ] )];
   univ := UnivariatePolynomial(sys, vars);
   return SerializeEvents(GenerateEvents(univ, [i]));
  end proc:
  map[inplace](proc(x) ArrayTools:-Extend(result, x, inplace=true) end proc,
                                             [Grid:-Seq(s(i, vars), i=1..nops(Q))]);
  return ReconstructEvents(result);
end proc:

# Procedure: ComputeEventsBType
#   Compute events such that intersection of two quadrics is tangent to sweeping plane.
#
# Parameters:
#   dir        - a direction of a gradient product it should
#                be the same as director of sweep
#   Q          - a set of conics
#   vars       - a list of variables
#
# Output:
#   Indexes of quadrics which intersect and a component of a vector product of
#   their gradients in given direction have a common root.
ComputeEventsBTypeGrid := proc( Q, dir::integer, vars::list )
  local s, result := Array([]);
  if nops(vars) < 3 then
    error "Expected at least three a variables!";
  fi;
  s := proc(i, j, vars::list)
    local p, prod, univ, sys:
    prod := LinearAlgebra:-CrossProduct( VectorCalculus:-Gradient( Q[i], vars ),
                                    VectorCalculus:-Gradient( Q[j], vars ) )[dir]:
    sys := [Q[i], Q[j], prod];
    univ := UnivariatePolynomial(sys, vars);
    return SerializeEvents(GenerateEvents(univ, [i, j]));
  end proc:
  map[inplace](proc(x) ArrayTools:-Extend(result, x, inplace=true) end proc,
                                   [Grid:-Seq(seq(s(i, j, vars), j=i+1..nops(Q)), i=1..nops(Q))]);
  return ReconstructEvents(result);
end proc;

# Procedure: ComputeEventsCTypeGrid
#   Compute events such that three quadrics intersects in a point.
#
# Parameters:
#   Q          - a set of quadrics
#   vars       - a list of variables
#
# Output:
#   Indexes of quadrics which intersect in a point.
ComputeEventsCTypeGrid := proc( Q, vars::list )
  uses ArrayTools;
  local result := Array([]), s;
  if nops(vars) < 3 then
    error "Expected at least three a variables!";
  fi;
  s := proc (i, j, k, vars::list)
    local univ, sys;
    sys := [Q[i], Q[j], Q[k]];
    univ := UnivariatePolynomial(sys, vars);
    return SerializeEvents(GenerateEvents(univ, [i, j, k]));
  end proc;
  map[inplace](proc(x) Extend(result, x, inplace=true) end proc, [Grid:-Seq(seq(seq(s(i, j, k, vars),
                                       k=j+1..nops(Q)),j=i+1..nops(Q)),i=1..nops(Q))]);
  return ReconstructEvents(result);
end proc:

# Procedure: ComputeAsymptoticAAEvents
#   Compute real algebraic numbers which corresponds to
#   asymptotic cases given by one quadrics.
#
# Parameters:
#   Q          - a set of quadrics
#   vars       - a list of variables
#
# Output:
#   A list of real algebraic numbers and indexes of quadrics -- events.
ComputeAsymptoticAAEvents:=proc(Q, vars::list)
  uses ArrayTools;
  local result := Array([]), s;
  if nops(vars) < 3 then
    error "Expected at least three a variables!";
  fi;
  s:=proc(i::integer, vars::list)
    local events := Array([]), sol, rootsF, tmp:
    rootsF := IsAsymptotic(Q[i], vars):
    for sol in rootsF do
      sol := op(sol);
      if not type(rhs(sol), rational) then
        error "Irrational asymptotic case! Are you sure the input is a set of quadrics?"
      fi:
      ArrayTools:-Append(events, EventType(RealAlgebraicNumber(lhs(sol) * denom(rhs(sol)) -
      numer(rhs(sol)), rhs(sol), rhs(sol)), [i]));
    od:
    return events;
  end proc;
  map[inplace](proc(x) Extend(result, x, inplace=true) end proc, [seq(s(i, vars), i=1..nops(Q))]);
  return result;
end proc;

# Procedure: ComputeAsymptoticABEvents
#   Compute real algebraic numbers which corresponds to
#   asymptotic cases given by one quadrics.
#
# Parameters:
#   Q          - a set of quadrics
#   vars       - a list of variables
#
# Output:
#   A list of real algebraic numbers and indexes of quadrics -- events.
ComputeAsymptoticABEventsGrid:=proc(Q, vars::list)
  uses ArrayTools;
  local result := Array([]), s;
  if nops(vars) < 3 then
    error "Expected at least three a variables!";
  fi;
  s:=proc(i::integer, j::integer, vars::list)
   local poly;
   poly := RigidMotionsParameterSpaceDecompostion:-IsAsymptoticIntersection(Q[i], Q[j], vars);
   if poly = NULL or nops(poly) = 0 then
     return [];
   fi;
   return SerializeEvents(GenerateEvents(poly, [i, j]));
  end proc;
  map[inplace](proc(x) Extend(result, x, inplace=true) end proc, [Grid:-Seq(seq(s(i, j, vars),
  j=i+1..nops(Q)), i=1..nops(Q))]);
  return ReconstructEvents(result);
end proc;

# Procedure: ComputeEventsFromAlgebraicNumbers
#   Compute and sort events
#
# Parameters:
#   Q     - set of quadrics or conics
#   vars       - a list of variables
# Output:
#   Sorted set of events
ComputeEventsFromAlgebraicNumbers := proc( Q, vars::list )
  local events := Array([]);
  ArrayTools:-Extend(events, ComputeEventsATypeGrid( Q, [2, 3], vars ), inplace=true);
  ArrayTools:-Extend(events, ComputeEventsBTypeGrid( Q, 1, vars ), inplace=true);
  ArrayTools:-Extend(events, ComputeEventsCTypeGrid( Q, vars ), inplace=true);
  ArrayTools:-Extend(events, ComputeAsymptoticAAEvents(Q, vars), inplace=true);
  ArrayTools:-Extend(events, ComputeAsymptoticABEventsGrid(Q, vars), inplace=true);
  return AlgebraicSort(events);
end proc:

# Procedure: LaunchComputeEvents
#   Computes sorted list of events for rotational part of rigid motions using the grid framework
#
#
# Parameters:
#   variables     - list of variables in which the problem is expressed
#   databasePath  - a path to a copy of the database CompRegister.db
#   nType         - neighborhood type: N1, N2 or N3.
#   kRange        - range of grid lines passed as a list
# Output:
#   It populates a database given by databasePath.
LaunchComputeEvents := proc(variables::list, databasePath::string, nType::string, kRange::list)
  local Q, Q1, Q2, Q3, R, i, db := Object(ComputationRegister, databasePath);
  vars := variables;
  dbPath:=databasePath;
  R := CayleyTransform(variables);
  Q1 := ComputeSetOfQuadrics(R, nType, 1, kRange);
  Q2 := map2(subs, [a = b, b = c, c = a], Q1);
  Q3 := map2(subs, [a = b, b = c, c = a], Q2);
  Q := [op(Q1), op(Q2), op(Q3), op(variables)];
  for i from 1 to nops(Q) do
    InsertQuadric(db, i, Q[i]);
  od;
  SynchronizeQuadrics(db);
  events := ComputeEventsFromAlgebraicNumbers(Q, variables);
  events := select[flatten](proc(x) evalb(GetInterval(GetRealAlgebraicNumber(x))[2] >= 0) end proc,
                                                                                events);
  events := ReduceEvents(events);
  AdjustEvents(events, upperbound(Q), variables);
  #Insert events into the register
  for i from 1 to upperbound(events) do
    InsertEvent(db, i, events[i]);
  od;
  SynchronizeEvents(db);
  Close(db);
end proc:

# Procedure: ComputeSamplePoints
#   Computes sample points for rotational part of rigid motions
#
#
# Parameters:
#   Q                  - list of quadrics
#   events             - an array of events
#   first              - integer value which indicates a first event to proceed.
#   last               - integer value which indicates a last event to proceed.
#   vars               - list of variables in which conics are expressed
#   db                 - an instance of the class ComputationRegister
#   skipped            - a list of the events' indices to be skipped
#
# Output:
#   It populates a database, given by databasePath, with sample points.
ComputeSamplePoints := proc (Q, events::Array, first::integer, last::integer,
                             vars::list, db::ComputationRegister, skipped::list:=[])
local i, midpoint, sys, samplePoints, disjointEvent:=[], ranumI, ranumJ;
  if first < 0 or last < 0 or last < first or upperbound(events) <= last then
    error "Bounds of the list of the events range are incorrect.":
  end if:
  for i from first to last do
    if i in skipped then
      next;
    fi:
    sys := Q[GetQuadrics(events[i])];
    ranumI := GetRealAlgebraicNumber(events[i]);
    ranumJ := GetRealAlgebraicNumber(events[i+1]);
    disjointEvent:=DisjointRanges(ranumI, ranumJ);
    midpoint := (GetInterval(disjointEvent[1])[2] + GetInterval(disjointEvent[2])[1])/2:
    # never call eval with sets!!
    sys := eval(sys, vars[1] = midpoint);
    LaunchComputeSamplePoints2D(sys, midpoint, 1, false, vars[2..], db);
    SynchronizeSamplePoints(db);
    InsertComputedNumber(db, i);
  end do;
end proc:

# Procedure: ParallelComputeSamplePoints
#   Computes sample points for rotational part of rigid motions. It should be call via Grid
#   framework.
ParallelComputeSamplePoints := proc()
  local me, n, events, skipped, first, last, offset, noEvents;
  local db:=Object(ComputationRegister, dbPath);
  me := Grid:-MyNode();
  skipped := FetchComputedNumbers(db);
  offset := FetchLowerEventID(db) - 1;
  noEvents := NumberOfEvents(db);
  # events-1 because the last event is a copy of events[-2]
  n := trunc((noEvents-1) / Grid:-NumNodes());
  first := offset + me * n + 1; last := offset + (me + 1) * n;
  if me = Grid:-NumNodes() - 1 then
    last := last + ((noEvents + offset) - (last + 1));
  fi;
  # recreate events
  events := FetchEvents(db, first, last + 1);
  RigidMotionsParameterSpaceDecompostion:-ComputeSamplePoints(Q, events, first , last, vars, db, skipped);
  Close(db);
  Grid:-Barrier();
end proc:

# Procedure: LaunchComputeSamplePoints
#   Resumes computations of sample points for rotational part of rigid motions using
#   the grid framework.
#
#
# Parameters:
#   variables     - list of variables in which the problem is expressed
#   databasePath  - a path to a database file. If file does not exist it will be crated.
#   nType         - neighborhood type: N1, N2 or N3.
#   nodes         - number of nodes used in the parallel computations
# Output:
#   It populates a database given by databasePath.
LaunchComputeSamplePoints := proc(variables::list, databasePath::string, nType::string,
                                 nodes:=kernelopts(numcpus))
  local db:=Object(ComputationRegister, databasePath);
  vars:=variables;
  dbPath:=databasePath;
  Q := FetchQuadrics(db);
  Close(db);
  Grid:-Setup("local");
  Grid:-Launch(RigidMotionsParameterSpaceDecompostion:-ParallelComputeSamplePoints,
               imports=['Q', 'vars', 'dbPath'], numnodes=nodes, allexternal=false);
end proc;

end module:


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

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

(* type: work_gga_x *)
(* prefix:
  gga_k_dk_params *params;

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

f := x ->
  add(1*params_a_aa[i]*x^(2*(i-1)), i=1..5) /
  add(1*params_a_bb[i]*x^(2*(i-1)), i=1..5):


Q := [b*c+a, a*c-b, b*c-2*c^2+a-2, b*c-c^2+a-1, b*c+c^2+a+1, b*c+2*c^2+a+2, 2*
b*c-c^2+2*a-1, 2*b*c+c^2+2*a+1, a*c-2*c^2-b-2, a*c-c^2-b-1, a*c+c^2-b+1, a*c+2
*c^2-b+2, 2*a*c-c^2-2*b-1, 2*a*c+c^2-2*b+1, a*c-2*b*c-2*a-b, a*c-b*c-a-b, a*c+
b*c+a-b, a*c+2*b*c+2*a-b, 2*a*c-b*c-a-2*b, 2*a*c+b*c+a-2*b, a^2+b^2-3*c^2-3, a
^2+b^2-c^2-1, 3*a^2+3*b^2-c^2-1, a^2+b^2-2*b*c-2*a, a^2+b^2-b*c-a, a^2+b^2+b*c
+a, a^2+b^2+2*b*c+2*a, 2*a^2+2*b^2-b*c-a, 2*a^2+2*b^2+b*c+a, a^2-2*a*c+b^2+2*b
, a^2-a*c+b^2+b, a^2+a*c+b^2-b, a^2+2*a*c+b^2-2*b, 2*a^2-a*c+2*b^2+b, 2*a^2+a*
c+2*b^2-b, a*c-2*b*c-c^2-2*a-b-1, a*c-2*b*c+c^2-2*a-b+1, a*c-b*c-2*c^2-a-b-2,
a*c-b*c-c^2-a-b-1, a*c-b*c+c^2-a-b+1, a*c-b*c+2*c^2-a-b+2, a*c+b*c-2*c^2+a-b-2
, a*c+b*c-c^2+a-b-1, a*c+b*c+c^2+a-b+1, a*c+b*c+2*c^2+a-b+2, a*c+2*b*c-c^2+2*a
-b-1, a*c+2*b*c+c^2+2*a-b+1, 2*a*c-b*c-c^2-a-2*b-1, 2*a*c-b*c+c^2-a-2*b+1, 2*a
*c+b*c-c^2+a-2*b-1, 2*a*c+b*c+c^2+a-2*b+1, a^2+b^2-4*b*c-3*c^2-4*a-3, a^2+b^2-\
4*b*c-c^2-4*a-1, a^2+b^2-4*b*c+c^2-4*a+1, a^2+b^2-4*b*c+3*c^2-4*a+3, a^2+b^2-2
*b*c-3*c^2-2*a-3, a^2+b^2-2*b*c-c^2-2*a-1, a^2+b^2-b*c-c^2-a-1, a^2+b^2+b*c-c^
2+a-1, a^2+b^2+2*b*c-3*c^2+2*a-3, a^2+b^2+2*b*c-c^2+2*a-1, a^2+b^2+4*b*c-3*c^2
+4*a-3, a^2+b^2+4*b*c-c^2+4*a-1, a^2+b^2+4*b*c+c^2+4*a+1, a^2+b^2+4*b*c+3*c^2+
4*a+3, 3*a^2+3*b^2-4*b*c-c^2-4*a-1, 3*a^2+3*b^2-4*b*c+c^2-4*a+1, 3*a^2+3*b^2-2
*b*c-c^2-2*a-1, 3*a^2+3*b^2+2*b*c-c^2+2*a-1, 3*a^2+3*b^2+4*b*c-c^2+4*a-1, 3*a^
2+3*b^2+4*b*c+c^2+4*a+1, a^2-4*a*c+b^2-3*c^2+4*b-3, a^2-4*a*c+b^2-c^2+4*b-1, a
^2-4*a*c+b^2+c^2+4*b+1, a^2-4*a*c+b^2+3*c^2+4*b+3, a^2-2*a*c+b^2-3*c^2+2*b-3,
a^2-2*a*c+b^2-c^2+2*b-1, a^2-a*c+b^2-c^2+b-1, a^2+a*c+b^2-c^2-b-1, a^2+2*a*c+b
^2-3*c^2-2*b-3, a^2+2*a*c+b^2-c^2-2*b-1, a^2+4*a*c+b^2-3*c^2-4*b-3, a^2+4*a*c+
b^2-c^2-4*b-1, a^2+4*a*c+b^2+c^2-4*b+1, a^2+4*a*c+b^2+3*c^2-4*b+3, 3*a^2-4*a*c
+3*b^2-c^2+4*b-1, 3*a^2-4*a*c+3*b^2+c^2+4*b+1, 3*a^2-2*a*c+3*b^2-c^2+2*b-1, 3*
a^2+2*a*c+3*b^2-c^2-2*b-1, 3*a^2+4*a*c+3*b^2-c^2-4*b-1, 3*a^2+4*a*c+3*b^2+c^2-\
4*b+1, a^2-2*a*c+b^2-b*c-a+2*b, a^2-2*a*c+b^2+b*c+a+2*b, a^2-a*c+b^2-2*b*c-2*a
+b, a^2-a*c+b^2-b*c-a+b, a^2-a*c+b^2+b*c+a+b, a^2-a*c+b^2+2*b*c+2*a+b, a^2+a*c
+b^2-2*b*c-2*a-b, a^2+a*c+b^2-b*c-a-b, a^2+a*c+b^2+b*c+a-b, a^2+a*c+b^2+2*b*c+
2*a-b, a^2+2*a*c+b^2-b*c-a-2*b, a^2+2*a*c+b^2+b*c+a-2*b, 2*a^2-a*c+2*b^2-b*c-a
+b, 2*a^2-a*c+2*b^2+b*c+a+b, 2*a^2+a*c+2*b^2-b*c-a-b, 2*a^2+a*c+2*b^2+b*c+a-b,
a^2-4*a*c+b^2-4*b*c+c^2-4*a+4*b+1, a^2-4*a*c+b^2-2*b*c-c^2-2*a+4*b-1, a^2-4*a*
c+b^2-2*b*c+c^2-2*a+4*b+1, a^2-4*a*c+b^2-2*b*c+3*c^2-2*a+4*b+3, a^2-4*a*c+b^2+
2*b*c-c^2+2*a+4*b-1, a^2-4*a*c+b^2+2*b*c+c^2+2*a+4*b+1, a^2-4*a*c+b^2+2*b*c+3*
c^2+2*a+4*b+3, a^2-4*a*c+b^2+4*b*c+c^2+4*a+4*b+1, a^2-2*a*c+b^2-4*b*c-c^2-4*a+
2*b-1, a^2-2*a*c+b^2-4*b*c+c^2-4*a+2*b+1, a^2-2*a*c+b^2-4*b*c+3*c^2-4*a+2*b+3,
a^2-2*a*c+b^2-2*b*c-3*c^2-2*a+2*b-3, a^2-2*a*c+b^2-2*b*c-c^2-2*a+2*b-1, a^2-2*
a*c+b^2-2*b*c+c^2-2*a+2*b+1, a^2-2*a*c+b^2-b*c+c^2-a+2*b+1, a^2-2*a*c+b^2+b*c+
c^2+a+2*b+1, a^2-2*a*c+b^2+2*b*c-3*c^2+2*a+2*b-3, a^2-2*a*c+b^2+2*b*c-c^2+2*a+
2*b-1, a^2-2*a*c+b^2+2*b*c+c^2+2*a+2*b+1, a^2-2*a*c+b^2+4*b*c-c^2+4*a+2*b-1, a
^2-2*a*c+b^2+4*b*c+c^2+4*a+2*b+1, a^2-2*a*c+b^2+4*b*c+3*c^2+4*a+2*b+3, a^2-a*c
+b^2-2*b*c+c^2-2*a+b+1, a^2-a*c+b^2-b*c-c^2-a+b-1, a^2-a*c+b^2+b*c-c^2+a+b-1,
a^2-a*c+b^2+2*b*c+c^2+2*a+b+1, a^2+a*c+b^2-2*b*c+c^2-2*a-b+1, a^2+a*c+b^2-b*c-
c^2-a-b-1, a^2+a*c+b^2+b*c-c^2+a-b-1, a^2+a*c+b^2+2*b*c+c^2+2*a-b+1, a^2+2*a*c
+b^2-4*b*c-c^2-4*a-2*b-1, a^2+2*a*c+b^2-4*b*c+c^2-4*a-2*b+1, a^2+2*a*c+b^2-4*b
*c+3*c^2-4*a-2*b+3, a^2+2*a*c+b^2-2*b*c-3*c^2-2*a-2*b-3, a^2+2*a*c+b^2-2*b*c-c
^2-2*a-2*b-1, a^2+2*a*c+b^2-2*b*c+c^2-2*a-2*b+1, a^2+2*a*c+b^2-b*c+c^2-a-2*b+1
, a^2+2*a*c+b^2+b*c+c^2+a-2*b+1, a^2+2*a*c+b^2+2*b*c-3*c^2+2*a-2*b-3, a^2+2*a*
c+b^2+2*b*c-c^2+2*a-2*b-1, a^2+2*a*c+b^2+2*b*c+c^2+2*a-2*b+1, a^2+2*a*c+b^2+4*
b*c-c^2+4*a-2*b-1, a^2+2*a*c+b^2+4*b*c+c^2+4*a-2*b+1, a^2+2*a*c+b^2+4*b*c+3*c^
2+4*a-2*b+3, a^2+4*a*c+b^2-4*b*c+c^2-4*a-4*b+1, a^2+4*a*c+b^2-2*b*c-c^2-2*a-4*
b-1, a^2+4*a*c+b^2-2*b*c+c^2-2*a-4*b+1, a^2+4*a*c+b^2-2*b*c+3*c^2-2*a-4*b+3, a
^2+4*a*c+b^2+2*b*c-c^2+2*a-4*b-1, a^2+4*a*c+b^2+2*b*c+c^2+2*a-4*b+1, a^2+4*a*c
+b^2+2*b*c+3*c^2+2*a-4*b+3, a^2+4*a*c+b^2+4*b*c+c^2+4*a-4*b+1, 3*a^2-4*a*c+3*b
^2-2*b*c+c^2-2*a+4*b+1, 3*a^2-4*a*c+3*b^2+2*b*c+c^2+2*a+4*b+1, 3*a^2-2*a*c+3*b
^2-4*b*c+c^2-4*a+2*b+1, 3*a^2-2*a*c+3*b^2-2*b*c-c^2-2*a+2*b-1, 3*a^2-2*a*c+3*b
^2+2*b*c-c^2+2*a+2*b-1, 3*a^2-2*a*c+3*b^2+4*b*c+c^2+4*a+2*b+1, 3*a^2+2*a*c+3*b
^2-4*b*c+c^2-4*a-2*b+1, 3*a^2+2*a*c+3*b^2-2*b*c-c^2-2*a-2*b-1, 3*a^2+2*a*c+3*b
^2+2*b*c-c^2+2*a-2*b-1, 3*a^2+2*a*c+3*b^2+4*b*c+c^2+4*a-2*b+1, 3*a^2+4*a*c+3*b
^2-2*b*c+c^2-2*a-4*b+1, 3*a^2+4*a*c+3*b^2+2*b*c+c^2+2*a-4*b+1, -b*c+a, a*b+c,
-2*b^2-b*c+a-2, -b^2-2*b*c+2*a-1, -b^2-b*c+a-1, b^2-2*b*c+2*a+1, b^2-b*c+a+1,
2*b^2-b*c+a+2, a*b-2*b*c+2*a+c, a*b-b*c+a+c, a*b+b*c-a+c, a*b+2*b*c-2*a+c, 2*a
*b-b*c+a+2*c, 2*a*b+b*c-a+2*c, a*b-2*b^2+c-2, a*b-b^2+c-1, a*b+b^2+c+1, a*b+2*
b^2+c+2, 2*a*b-b^2+2*c-1, 2*a*b+b^2+2*c+1, a^2-2*b*c+c^2+2*a, a^2-b*c+c^2+a, a
^2+b*c+c^2-a, a^2+2*b*c+c^2-2*a, 2*a^2-b*c+2*c^2+a, 2*a^2+b*c+2*c^2-a, a^2-3*b
^2+c^2-3, a^2-b^2+c^2-1, 3*a^2-b^2+3*c^2-1, a^2-2*a*b+c^2-2*c, a^2-a*b+c^2-c,
a^2+a*b+c^2+c, a^2+2*a*b+c^2+2*c, 2*a^2-a*b+2*c^2-c, 2*a^2+a*b+2*c^2+c, a*b-2*
b^2-b*c+a+c-2, a*b-2*b^2+b*c-a+c-2, a*b-b^2-2*b*c+2*a+c-1, a*b-b^2-b*c+a+c-1,
a*b-b^2+b*c-a+c-1, a*b-b^2+2*b*c-2*a+c-1, a*b+b^2-2*b*c+2*a+c+1, a*b+b^2-b*c+a
+c+1, a*b+b^2+b*c-a+c+1, a*b+b^2+2*b*c-2*a+c+1, a*b+2*b^2-b*c+a+c+2, a*b+2*b^2
+b*c-a+c+2, 2*a*b-b^2-b*c+a+2*c-1, 2*a*b-b^2+b*c-a+2*c-1, 2*a*b+b^2-b*c+a+2*c+
1, 2*a*b+b^2+b*c-a+2*c+1, a^2-3*b^2-4*b*c+c^2+4*a-3, a^2-3*b^2-2*b*c+c^2+2*a-3
, a^2-3*b^2+2*b*c+c^2-2*a-3, a^2-3*b^2+4*b*c+c^2-4*a-3, a^2-b^2-4*b*c+c^2+4*a-\
1, a^2-b^2-2*b*c+c^2+2*a-1, a^2-b^2-b*c+c^2+a-1, a^2-b^2+b*c+c^2-a-1, a^2-b^2+
2*b*c+c^2-2*a-1, a^2-b^2+4*b*c+c^2-4*a-1, a^2+b^2-4*b*c+c^2+4*a+1, a^2+b^2+4*b
*c+c^2-4*a+1, a^2+3*b^2-4*b*c+c^2+4*a+3, a^2+3*b^2+4*b*c+c^2-4*a+3, 3*a^2-b^2-\
4*b*c+3*c^2+4*a-1, 3*a^2-b^2-2*b*c+3*c^2+2*a-1, 3*a^2-b^2+2*b*c+3*c^2-2*a-1, 3
*a^2-b^2+4*b*c+3*c^2-4*a-1, 3*a^2+b^2-4*b*c+3*c^2+4*a+1, 3*a^2+b^2+4*b*c+3*c^2
-4*a+1, a^2-2*a*b-b*c+c^2+a-2*c, a^2-2*a*b+b*c+c^2-a-2*c, a^2-a*b-2*b*c+c^2+2*
a-c, a^2-a*b-b*c+c^2+a-c, a^2-a*b+b*c+c^2-a-c, a^2-a*b+2*b*c+c^2-2*a-c, a^2+a*
b-2*b*c+c^2+2*a+c, a^2+a*b-b*c+c^2+a+c, a^2+a*b+b*c+c^2-a+c, a^2+a*b+2*b*c+c^2
-2*a+c, a^2+2*a*b-b*c+c^2+a+2*c, a^2+2*a*b+b*c+c^2-a+2*c, 2*a^2-a*b-b*c+2*c^2+
a-c, 2*a^2-a*b+b*c+2*c^2-a-c, 2*a^2+a*b-b*c+2*c^2+a+c, 2*a^2+a*b+b*c+2*c^2-a+c
, a^2-4*a*b-3*b^2+c^2-4*c-3, a^2-4*a*b-b^2+c^2-4*c-1, a^2-4*a*b+b^2+c^2-4*c+1,
a^2-4*a*b+3*b^2+c^2-4*c+3, a^2-2*a*b-3*b^2+c^2-2*c-3, a^2-2*a*b-b^2+c^2-2*c-1,
a^2-a*b-b^2+c^2-c-1, a^2+a*b-b^2+c^2+c-1, a^2+2*a*b-3*b^2+c^2+2*c-3, a^2+2*a*b
-b^2+c^2+2*c-1, a^2+4*a*b-3*b^2+c^2+4*c-3, a^2+4*a*b-b^2+c^2+4*c-1, a^2+4*a*b+
b^2+c^2+4*c+1, a^2+4*a*b+3*b^2+c^2+4*c+3, 3*a^2-4*a*b-b^2+3*c^2-4*c-1, 3*a^2-4
*a*b+b^2+3*c^2-4*c+1, 3*a^2-2*a*b-b^2+3*c^2-2*c-1, 3*a^2+2*a*b-b^2+3*c^2+2*c-1
, 3*a^2+4*a*b-b^2+3*c^2+4*c-1, 3*a^2+4*a*b+b^2+3*c^2+4*c+1, a^2-4*a*b-b^2-2*b*
c+c^2+2*a-4*c-1, a^2-4*a*b-b^2+2*b*c+c^2-2*a-4*c-1, a^2-4*a*b+b^2-4*b*c+c^2+4*
a-4*c+1, a^2-4*a*b+b^2-2*b*c+c^2+2*a-4*c+1, a^2-4*a*b+b^2+2*b*c+c^2-2*a-4*c+1,
a^2-4*a*b+b^2+4*b*c+c^2-4*a-4*c+1, a^2-4*a*b+3*b^2-2*b*c+c^2+2*a-4*c+3, a^2-4*
a*b+3*b^2+2*b*c+c^2-2*a-4*c+3, a^2-2*a*b-3*b^2-2*b*c+c^2+2*a-2*c-3, a^2-2*a*b-\
3*b^2+2*b*c+c^2-2*a-2*c-3, a^2-2*a*b-b^2-4*b*c+c^2+4*a-2*c-1, a^2-2*a*b-b^2-2*
b*c+c^2+2*a-2*c-1, a^2-2*a*b-b^2+2*b*c+c^2-2*a-2*c-1, a^2-2*a*b-b^2+4*b*c+c^2-\
4*a-2*c-1, a^2-2*a*b+b^2-4*b*c+c^2+4*a-2*c+1, a^2-2*a*b+b^2-2*b*c+c^2+2*a-2*c+
1, a^2-2*a*b+b^2-b*c+c^2+a-2*c+1, a^2-2*a*b+b^2+b*c+c^2-a-2*c+1, a^2-2*a*b+b^2
+2*b*c+c^2-2*a-2*c+1, a^2-2*a*b+b^2+4*b*c+c^2-4*a-2*c+1, a^2-2*a*b+3*b^2-4*b*c
+c^2+4*a-2*c+3, a^2-2*a*b+3*b^2+4*b*c+c^2-4*a-2*c+3, a^2-a*b-b^2-b*c+c^2+a-c-1
, a^2-a*b-b^2+b*c+c^2-a-c-1, a^2-a*b+b^2-2*b*c+c^2+2*a-c+1, a^2-a*b+b^2+2*b*c+
c^2-2*a-c+1, a^2+a*b-b^2-b*c+c^2+a+c-1, a^2+a*b-b^2+b*c+c^2-a+c-1, a^2+a*b+b^2
-2*b*c+c^2+2*a+c+1, a^2+a*b+b^2+2*b*c+c^2-2*a+c+1, a^2+2*a*b-3*b^2-2*b*c+c^2+2
*a+2*c-3, a^2+2*a*b-3*b^2+2*b*c+c^2-2*a+2*c-3, a^2+2*a*b-b^2-4*b*c+c^2+4*a+2*c
-1, a^2+2*a*b-b^2-2*b*c+c^2+2*a+2*c-1, a^2+2*a*b-b^2+2*b*c+c^2-2*a+2*c-1, a^2+
2*a*b-b^2+4*b*c+c^2-4*a+2*c-1, a^2+2*a*b+b^2-4*b*c+c^2+4*a+2*c+1, a^2+2*a*b+b^
2-2*b*c+c^2+2*a+2*c+1, a^2+2*a*b+b^2-b*c+c^2+a+2*c+1, a^2+2*a*b+b^2+b*c+c^2-a+
2*c+1, a^2+2*a*b+b^2+2*b*c+c^2-2*a+2*c+1, a^2+2*a*b+b^2+4*b*c+c^2-4*a+2*c+1, a
^2+2*a*b+3*b^2-4*b*c+c^2+4*a+2*c+3, a^2+2*a*b+3*b^2+4*b*c+c^2-4*a+2*c+3, a^2+4
*a*b-b^2-2*b*c+c^2+2*a+4*c-1, a^2+4*a*b-b^2+2*b*c+c^2-2*a+4*c-1, a^2+4*a*b+b^2
-4*b*c+c^2+4*a+4*c+1, a^2+4*a*b+b^2-2*b*c+c^2+2*a+4*c+1, a^2+4*a*b+b^2+2*b*c+c
^2-2*a+4*c+1, a^2+4*a*b+b^2+4*b*c+c^2-4*a+4*c+1, a^2+4*a*b+3*b^2-2*b*c+c^2+2*a
+4*c+3, a^2+4*a*b+3*b^2+2*b*c+c^2-2*a+4*c+3, 3*a^2-4*a*b+b^2-2*b*c+3*c^2+2*a-4
*c+1, 3*a^2-4*a*b+b^2+2*b*c+3*c^2-2*a-4*c+1, 3*a^2-2*a*b-b^2-2*b*c+3*c^2+2*a-2
*c-1, 3*a^2-2*a*b-b^2+2*b*c+3*c^2-2*a-2*c-1, 3*a^2-2*a*b+b^2-4*b*c+3*c^2+4*a-2
*c+1, 3*a^2-2*a*b+b^2+4*b*c+3*c^2-4*a-2*c+1, 3*a^2+2*a*b-b^2-2*b*c+3*c^2+2*a+2
*c-1, 3*a^2+2*a*b-b^2+2*b*c+3*c^2-2*a+2*c-1, 3*a^2+2*a*b+b^2-4*b*c+3*c^2+4*a+2
*c+1, 3*a^2+2*a*b+b^2+4*b*c+3*c^2-4*a+2*c+1, 3*a^2+4*a*b+b^2-2*b*c+3*c^2+2*a+4
*c+1, 3*a^2+4*a*b+b^2+2*b*c+3*c^2-2*a+4*c+1, a*c+b, a*b-c, a*c-2*b^2-2*c^2+b,
a*c-b^2-c^2+b, a*c+b^2+c^2+b, a*c+2*b^2+2*c^2+b, 2*a*c-b^2-c^2+2*b, 2*a*c+b^2+
c^2+2*b, a*b-2*b^2-2*c^2-c, a*b-b^2-c^2-c, a*b+b^2+c^2-c, a*b+2*b^2+2*c^2-c, 2
*a*b-b^2-c^2-2*c, 2*a*b+b^2+c^2-2*c, a*b-2*a*c-2*b-c, a*b-a*c-b-c, a*b+a*c+b-c
, a*b+2*a*c+2*b-c, 2*a*b-a*c-b-2*c, 2*a*b+a*c+b-2*c, a^2-3*b^2-3*c^2+1, a^2-b^
2-c^2+1, 3*a^2-b^2-c^2+3, a^2-2*a*c-2*b+1, a^2-a*c-b+1, a^2+a*c+b+1, a^2+2*a*c
+2*b+1, 2*a^2-a*c-b+2, 2*a^2+a*c+b+2, a^2-2*a*b+2*c+1, a^2-a*b+c+1, a^2+a*b-c+
1, a^2+2*a*b-2*c+1, 2*a^2-a*b+c+2, 2*a^2+a*b-c+2, a*b-2*a*c-b^2-c^2-2*b-c, a*b
-2*a*c+b^2+c^2-2*b-c, a*b-a*c-2*b^2-2*c^2-b-c, a*b-a*c-b^2-c^2-b-c, a*b-a*c+b^
2+c^2-b-c, a*b-a*c+2*b^2+2*c^2-b-c, a*b+a*c-2*b^2-2*c^2+b-c, a*b+a*c-b^2-c^2+b
-c, a*b+a*c+b^2+c^2+b-c, a*b+a*c+2*b^2+2*c^2+b-c, a*b+2*a*c-b^2-c^2+2*b-c, a*b
+2*a*c+b^2+c^2+2*b-c, 2*a*b-a*c-b^2-c^2-b-2*c, 2*a*b-a*c+b^2+c^2-b-2*c, 2*a*b+
a*c-b^2-c^2+b-2*c, 2*a*b+a*c+b^2+c^2+b-2*c, a^2-4*a*c-3*b^2-3*c^2-4*b+1, a^2-4
*a*c-b^2-c^2-4*b+1, a^2-4*a*c+b^2+c^2-4*b+1, a^2-4*a*c+3*b^2+3*c^2-4*b+1, a^2-\
2*a*c-3*b^2-3*c^2-2*b+1, a^2-2*a*c-b^2-c^2-2*b+1, a^2-a*c-b^2-c^2-b+1, a^2+a*c
-b^2-c^2+b+1, a^2+2*a*c-3*b^2-3*c^2+2*b+1, a^2+2*a*c-b^2-c^2+2*b+1, a^2+4*a*c-\
3*b^2-3*c^2+4*b+1, a^2+4*a*c-b^2-c^2+4*b+1, a^2+4*a*c+b^2+c^2+4*b+1, a^2+4*a*c
+3*b^2+3*c^2+4*b+1, 3*a^2-4*a*c-b^2-c^2-4*b+3, 3*a^2-4*a*c+b^2+c^2-4*b+3, 3*a^
2-2*a*c-b^2-c^2-2*b+3, 3*a^2+2*a*c-b^2-c^2+2*b+3, 3*a^2+4*a*c-b^2-c^2+4*b+3, 3
*a^2+4*a*c+b^2+c^2+4*b+3, a^2-4*a*b-3*b^2-3*c^2+4*c+1, a^2-4*a*b-b^2-c^2+4*c+1
, a^2-4*a*b+b^2+c^2+4*c+1, a^2-4*a*b+3*b^2+3*c^2+4*c+1, a^2-2*a*b-3*b^2-3*c^2+
2*c+1, a^2-2*a*b-b^2-c^2+2*c+1, a^2-a*b-b^2-c^2+c+1, a^2+a*b-b^2-c^2-c+1, a^2+
2*a*b-3*b^2-3*c^2-2*c+1, a^2+2*a*b-b^2-c^2-2*c+1, a^2+4*a*b-3*b^2-3*c^2-4*c+1,
a^2+4*a*b-b^2-c^2-4*c+1, a^2+4*a*b+b^2+c^2-4*c+1, a^2+4*a*b+3*b^2+3*c^2-4*c+1,
3*a^2-4*a*b-b^2-c^2+4*c+3, 3*a^2-4*a*b+b^2+c^2+4*c+3, 3*a^2-2*a*b-b^2-c^2+2*c+
3, 3*a^2+2*a*b-b^2-c^2-2*c+3, 3*a^2+4*a*b-b^2-c^2-4*c+3, 3*a^2+4*a*b+b^2+c^2-4
*c+3, a^2-2*a*b-a*c-b+2*c+1, a^2-2*a*b+a*c+b+2*c+1, a^2-a*b-2*a*c-2*b+c+1, a^2
-a*b-a*c-b+c+1, a^2-a*b+a*c+b+c+1, a^2-a*b+2*a*c+2*b+c+1, a^2+a*b-2*a*c-2*b-c+
1, a^2+a*b-a*c-b-c+1, a^2+a*b+a*c+b-c+1, a^2+a*b+2*a*c+2*b-c+1, a^2+2*a*b-a*c-
b-2*c+1, a^2+2*a*b+a*c+b-2*c+1, 2*a^2-a*b-a*c-b+c+2, 2*a^2-a*b+a*c+b+c+2, 2*a^
2+a*b-a*c-b-c+2, 2*a^2+a*b+a*c+b-c+2, a^2-4*a*b-4*a*c+b^2+c^2-4*b+4*c+1, a^2-4
*a*b-2*a*c-b^2-c^2-2*b+4*c+1, a^2-4*a*b-2*a*c+b^2+c^2-2*b+4*c+1, a^2-4*a*b-2*a
*c+3*b^2+3*c^2-2*b+4*c+1, a^2-4*a*b+2*a*c-b^2-c^2+2*b+4*c+1, a^2-4*a*b+2*a*c+b
^2+c^2+2*b+4*c+1, a^2-4*a*b+2*a*c+3*b^2+3*c^2+2*b+4*c+1, a^2-4*a*b+4*a*c+b^2+c
^2+4*b+4*c+1, a^2-2*a*b-4*a*c-b^2-c^2-4*b+2*c+1, a^2-2*a*b-4*a*c+b^2+c^2-4*b+2
*c+1, a^2-2*a*b-4*a*c+3*b^2+3*c^2-4*b+2*c+1, a^2-2*a*b-2*a*c-3*b^2-3*c^2-2*b+2
*c+1, a^2-2*a*b-2*a*c-b^2-c^2-2*b+2*c+1, a^2-2*a*b-2*a*c+b^2+c^2-2*b+2*c+1, a^
2-2*a*b-a*c+b^2+c^2-b+2*c+1, a^2-2*a*b+a*c+b^2+c^2+b+2*c+1, a^2-2*a*b+2*a*c-3*
b^2-3*c^2+2*b+2*c+1, a^2-2*a*b+2*a*c-b^2-c^2+2*b+2*c+1, a^2-2*a*b+2*a*c+b^2+c^
2+2*b+2*c+1, a^2-2*a*b+4*a*c-b^2-c^2+4*b+2*c+1, a^2-2*a*b+4*a*c+b^2+c^2+4*b+2*
c+1, a^2-2*a*b+4*a*c+3*b^2+3*c^2+4*b+2*c+1, a^2-a*b-2*a*c+b^2+c^2-2*b+c+1, a^2
-a*b-a*c-b^2-c^2-b+c+1, a^2-a*b+a*c-b^2-c^2+b+c+1, a^2-a*b+2*a*c+b^2+c^2+2*b+c
+1, a^2+a*b-2*a*c+b^2+c^2-2*b-c+1, a^2+a*b-a*c-b^2-c^2-b-c+1, a^2+a*b+a*c-b^2-
c^2+b-c+1, a^2+a*b+2*a*c+b^2+c^2+2*b-c+1, a^2+2*a*b-4*a*c-b^2-c^2-4*b-2*c+1, a
^2+2*a*b-4*a*c+b^2+c^2-4*b-2*c+1, a^2+2*a*b-4*a*c+3*b^2+3*c^2-4*b-2*c+1, a^2+2
*a*b-2*a*c-3*b^2-3*c^2-2*b-2*c+1, a^2+2*a*b-2*a*c-b^2-c^2-2*b-2*c+1, a^2+2*a*b
-2*a*c+b^2+c^2-2*b-2*c+1, a^2+2*a*b-a*c+b^2+c^2-b-2*c+1, a^2+2*a*b+a*c+b^2+c^2
+b-2*c+1, a^2+2*a*b+2*a*c-3*b^2-3*c^2+2*b-2*c+1, a^2+2*a*b+2*a*c-b^2-c^2+2*b-2
*c+1, a^2+2*a*b+2*a*c+b^2+c^2+2*b-2*c+1, a^2+2*a*b+4*a*c-b^2-c^2+4*b-2*c+1, a^
2+2*a*b+4*a*c+b^2+c^2+4*b-2*c+1, a^2+2*a*b+4*a*c+3*b^2+3*c^2+4*b-2*c+1, a^2+4*
a*b-4*a*c+b^2+c^2-4*b-4*c+1, a^2+4*a*b-2*a*c-b^2-c^2-2*b-4*c+1, a^2+4*a*b-2*a*
c+b^2+c^2-2*b-4*c+1, a^2+4*a*b-2*a*c+3*b^2+3*c^2-2*b-4*c+1, a^2+4*a*b+2*a*c-b^
2-c^2+2*b-4*c+1, a^2+4*a*b+2*a*c+b^2+c^2+2*b-4*c+1, a^2+4*a*b+2*a*c+3*b^2+3*c^
2+2*b-4*c+1, a^2+4*a*b+4*a*c+b^2+c^2+4*b-4*c+1, 3*a^2-4*a*b-2*a*c+b^2+c^2-2*b+
4*c+3, 3*a^2-4*a*b+2*a*c+b^2+c^2+2*b+4*c+3, 3*a^2-2*a*b-4*a*c+b^2+c^2-4*b+2*c+
3, 3*a^2-2*a*b-2*a*c-b^2-c^2-2*b+2*c+3, 3*a^2-2*a*b+2*a*c-b^2-c^2+2*b+2*c+3, 3
*a^2-2*a*b+4*a*c+b^2+c^2+4*b+2*c+3, 3*a^2+2*a*b-4*a*c+b^2+c^2-4*b-2*c+3, 3*a^2
+2*a*b-2*a*c-b^2-c^2-2*b-2*c+3, 3*a^2+2*a*b+2*a*c-b^2-c^2+2*b-2*c+3, 3*a^2+2*a
*b+4*a*c+b^2+c^2+4*b-2*c+3, 3*a^2+4*a*b-2*a*c+b^2+c^2-2*b-4*c+3, 3*a^2+4*a*b+2
*a*c+b^2+c^2+2*b-4*c+3];


################################################################
# Utility function for irregular term extraction
# Usage:
#
#    gencoeffs(a/x+c+b*x+a*x^3,x);
#
#     Returns ->>  [-1, a], [0, c], [1, b], [2, 0], [3, a]
#
#  (i.e. it returns a list of powers of x and their coefficients
################################################################

gencoeffs := proc(expr::algebraic, var::name)
   local i;
   if (expr <> 0 ) then
   return seq([i,frontend(coeff,[expr,var,i])],
       i=frontend(ldegree,[expr,var]) .. frontend(degree,[expr,var]));
   else
    return [0,0];
   end if;
end proc;

#######################################################
# List of grid functions symbols defined in the problem
# See BSSN_Spher.mw or BSSN_Spher.mpl
#######################################################

grid_functions:={alpha,K,beta,phi,Axx,Athth,A,B,metxx,metthetatheta,DDLxx,DDLthetatheta,rho,TS,Lamx,em4phi,RRxx,RRthetatheta,JSxx,JSthth,Uxx,Pxx,Uthetatheta,Pthetatheta,C5s,C1s,psi,Sx,PI2,PHI2,U,rPI,iPI,rPHI,iPHI,a1,rpsi,ipsi,rPIb2,iPIb2,b1,a2,rpsidot,ipsidot,C5psi,divbeta};

################################################################
# List of tensors, will be used for getting the RHS of equations
# and looping over all of the equations
################################################################

all_tensors:=[
      [EMPH,0,dn],
   [DPH,0,dn] , [dg,2,dn] ,
         [met,2,dn], [DDL,2,dn],
         [DB,0,dn],
   [DTK,0,dn],
         [RR,2,dn], [U,2,dn] , [P,2,dn],
   [DAA,2,dn],
   [DLAM,1,up],
       [C1,0,dn],
       [C5,0,dn],
   [HS,0,dn],
   [HS3,0,dn],
   [MK,1,up]
             ];

# List of coordinates used: (x is r in spherical coordinate here)
coord:=[x,theta,phi];

######################################################################
# The coordiante coefficients that are defined
# in the definition of the generic 3-metric in curvilinear coordinate.
######################################################################

coordcoef:=[1,x^2,x^2*sin(theta)^2];

####################################################################
# Initiating a table for the diff expression of tensors
# tt[A][all][x] will store the x compononent of tensor A
# tt[A][sep][x] will store the x component of tensor A in seperated
# powers of 'r'
# tt[A][reg][x] will store the regular part of above
# tt[A][dis][x] will become the finite difference approximation of
# the above. All in all, 'tt' is our working table of everything!
####################################################################

tt:=table([]):

#########################################
# All of the following atomic variables
# will be used for indexing in table 'tt'
# and shall not be assigned anythin
#########################################

unassign('all');
unassign('sep');
unassign('tres');
unassign('reg');
unassign('dis');
unassign('disreg');
unassign('disinf');
unassign('inf');

#######  IMPORTANT NOTE: ###############################
# Never use i,j,k as indexing as when working with FD as
# they are protected by default to be used as indexing
# of coordinates x, y , z
########################################################

for jj from 1 to nops(all_tensors) do

  # Tensor name
  nm:= all_tensors[jj][1]:

  # Tensor rank
  rank := all_tensors[jj][2]:

  # Tensor type
  tp  := all_tensors[jj][3]:

  # rank-1 tensors
  if rank = 1 then
  tt[nm][all] := table( [ seq( coord[ii] = expand(grcomponent(nm(tp),[coord[ii]])/coordcoef[ii])  ,ii=1..nops(coord)) ] ):
  end if;

  #Assuming rank-2 tensors are all diagnoal (it is the case in spherical symmetry)
  if rank = 2 then
  tt[nm][all] := table( [ seq( coord[ii] = expand(grcomponent(nm(tp,tp),[coord[ii],coord[ii]])/coordcoef[ii])  ,ii=1..nops(coord))  ] ):
  end if;

  if rank = 0 then
  tt[nm][all] := grcomponent(nm):
  end if;

  #Breaking down the expressions to regular and irregular parts

  if rank <> 0 then
  tt[nm][sep] := table (  [  seq( coord[ii] =  [ gencoeffs(tt[nm][all][coord[ii]],'x')   ]  , ii=1..nops(coord) )   ]  ) ;

  end if;

  if rank = 0 then
    tt[nm][sep] := [gencoeffs(tt[nm][all],'x')];
  end if;
end do:

# Reading EOM for matter and adding it to tt table
read("eq_evol_matter.mpl");

evol_matters := [ [EQRPIb2,0,dn] , [EQIPIb2,0,dn] , [EQRPSI,0,dn]  ,[EQIPSI,0,dn], [DRPSI,0,dn], [DIPSI,0,dn] ]:

tt[EQRPIb2][all] := rhs(  eq_evol_rPIb2  ):
tt[EQIPIb2][all] := rhs( eq_evol_iPIb2   ):
tt[EQRPSI][all] := rhs(eq_evol_rpsi):
tt[EQIPSI][all] := rhs(eq_evol_ipsi):
tt[DRPSI][all] := diff(rpsi(t,x),x)+myzero*x:
tt[DIPSI][all] := diff(ipsi(t,x),x)+myzero*x:

for jj from 1 to nops(evol_matters) do
  nm:= evol_matters[jj][1]:
  #Breaking down the expressions to regular and irregular parts
  tt[nm][sep] := [gencoeffs(tt[nm][all],'x')];
end do:

# Updating all_tensors with newly added tensors:

all_tensors:=[
      [EMPH,0,dn],
   [DPH,0,dn] , [dg,2,dn] ,
         [met,2,dn], [DDL,2,dn],
         [DB,0,dn],
   [DTK,0,dn],
         [RR,2,dn], [U,2,dn] , [P,2,dn],
   [DAA,2,dn],
   [DLAM,1,up],
       [C1,0,dn],
       [C5,0,dn],
   [HS,0,dn],
   [HS3,0,dn],
   [MK,1,up],
   [EQRPIb2,0,dn],
   [EQIPIb2,0,dn],
   [EQIPSI,0,dn],
   [EQRPSI,0,dn],
   [DRPSI,0,dn],
   [DIPSI,0,dn]
             ];

#Checking that gencoeffs generated coefficients correctly:
check_res := true;
if (check_res) then

		for jj from 1 to nops(all_tensors) do
		  nm:= all_tensors[jj][1]:
		  rank := all_tensors[jj][2]:
		  tp  := all_tensors[jj][3]:
		  if rank <> 0 then
			tt[nm][tres] := table ( [ seq (  coord[ii] = simplify(expand( tt[nm][all][coord[ii]] - sum(x^tt[nm][sep][coord[ii]][k][1]*tt[nm][sep][coord[ii]][k][2],k=1..nops(tt[nm][sep][coord[ii]])   ) ))      ,ii=1..nops(coord) ) ] );
		  else
			tt[nm][tres] := simplify(expand( tt[nm][all] - sum(x^tt[nm][sep][k][1]*tt[nm][sep][k][2],k=1..nops(tt[nm][sep])   ) ))
		  end if;
		end do:

		for jj from 1 to nops(all_tensors) do
		  nm:= all_tensors[jj][1]:
		  rank := all_tensors[jj][2]:
		  tp  := all_tensors[jj][3]:
		  if rank <> 0 then
			 for ii from 1 to nops(coord) do
			  print(tt[nm][tres][coord[ii]]);
			 end do
		  else
			  print(tt[nm][tres]);
		  end if;

		end do:
end if;

for jj from 1 to nops(all_tensors) do
  nm:= all_tensors[jj][1]:
  rank := all_tensors[jj][2]:
  tp  := all_tensors[jj][3]:
  if rank <> 0 then
    for ii from 1 to nops(coord) do
        if tt[nm][sep][coord[ii]][1][1] >= 0 then
               print(nm + comp + coord[ii] + reg );
        else
                print( nm + comp + coord[ii] + ireg);
                kk := 1;
                while ( tt[nm][sep][coord[ii]][kk][1] < 0 ) do
                   print( tt[nm][sep][coord[ii]][kk][2]*x^tt[nm][sep][coord[ii]][kk][1] );
                    kk := kk + 1;
                end do
        end if;
    end do;
  else
       if tt[nm][sep][1][1] >= 0 then
          print(nm + comp + reg);
       else
          print(nm + comp + ireg);
          kk := 1;
          while ( tt[nm][sep][kk][1] < 0 ) do
                   print( tt[nm][sep][kk][2]*x^tt[nm][sep][kk][1] );
                    kk := kk + 1;
          end do
        end if;
  end if;

end do:

# End of residual check

#### NOTE: ############################################
# Below was effectively disabled by setting all to 0
# i.e. we decided to leave the expressions as they are
#######################################################

# Table to regularize the negative powers of see the following
reg_tbl:= table( [
 [dg,theta,-1] = 0 ,
 [dg,phi,-1]   = 0 ,
 [DDL,theta,-1] = 0 ,
 [DDL,phi,-1]  = 0 ,
 [RR,x,-2] = 0 ,
 [RR,x,-1] = 0,
 [RR,theta,-2] = 0,
 [RR,theta,-1] = 0,
 [RR,phi,-2] = 0,
 [RR,phi,-1] = 0,
 [P,theta,-1] = 0,
 [P,phi,-1]   = 0,
 [DLAM,x,-2] = 0,
 [DLAM,x,-1] = 0,
 [C1,-1] = 0,
 [HS,-1] = 0,
 [HS3,-1] = 0,
 [EQRPIb2,-1] = 0 ,
 [EQIPIb2,-1] = 0,
 [MK,x,-1] = 0,
 [DB,-1] = 0
] );

#################################################
# Building list of regular terms in expressions:
# Irregular term replacement was disabled
#################################################
for ii from 1 to nops(all_tensors) do
  nm:= all_tensors[ii][1]:
  rank := all_tensors[ii][2]:
  tp  := all_tensors[ii][3]:
  if rank <> 0 then
    for jj from 1 to nops(coord) do
        if tt[nm][sep][coord[jj]][1][1] >= 0 then
           tt[nm][reg][coord[jj]] := tt[nm][all][coord[jj]];
        else
           acm := 0;
           for kk from 1 to nops( tt[nm][sep][coord[jj]] ) do
             ord :=  tt[nm][sep][coord[jj]][kk][1];
             trm :=  tt[nm][sep][coord[jj]][kk][2];
             if ord < 0 then
               if not(assigned(reg_tbl[ [nm,coord[jj],ord] ] ) ) then
                  print("uncomplete reg table for:");
                  print(nm + coord[jj] + ord);
                  error("quiting...");
               else
                  acm := acm + reg_tbl[ [nm,coord[jj],ord] ];
               end if;
             else
               acm := acm + trm*x^ord;
             end if;
           end do;
           tt[nm][reg][coord[jj]] := acm;
        end if
     end do;
  else

        if tt[nm][sep][1][1] >= 0 then
           tt[nm][reg] := tt[nm][all];
        else
           acm := 0;
           for kk from 1 to nops( tt[nm][sep] ) do
             ord :=  tt[nm][sep][kk][1];
             trm :=  tt[nm][sep][kk][2];
             if ord < 0 then
               if not(assigned(reg_tbl[ [nm,ord] ] ) ) then
                  print("uncomplete reg table for:");
                  print(nm + ord);
                  error("quitting...");
               else
                  acm := acm + reg_tbl[ [nm,ord] ];
               end if;
             else
               acm := acm + trm*x^ord;
             end if;
           end do;
           tt[nm][reg]:= acm;
        end if

  end if;
end do:

# Treating 3-Reimmenn tensor seperately

not_all_tensors:=[
         [RR,2,dn]
             ];

R_rest := table([]);
R_ireg := table([]);

for ii from 1 to nops(not_all_tensors) do
  nm:= not_all_tensors[ii][1]:
  rank := not_all_tensors[ii][2]:
  tp  := not_all_tensors[ii][3]:
  if rank <> 0 then
    for jj from 1 to nops(coord) do
        rr_ireg := 0;
        rr_rest := 0;
        for kk from 1 to nops( tt[nm][sep][coord[jj]] ) do
             ord :=  tt[nm][sep][coord[jj]][kk][1];
             trm :=  tt[nm][sep][coord[jj]][kk][2];
             if ord < 0 then
                  rr_ireg:= rr_ireg + trm*x^ord;
             else
                  rr_rest := rr_rest + trm*x^ord;
             end if;
           end do;
        R_ireg[coord[jj]] := rr_ireg;
        R_rest[coord[jj]] := rr_rest;
     end do;
  end if;
end do:

check := true;

#############################################
# Following will print the difference
# between all and reg aka the irregular terms
# in the tensor components
#############################################

if check then
 for ii from 1 to nops(all_tensors) do
  nm:= all_tensors[ii][1]:
  rank := all_tensors[ii][2]:
  tp  := all_tensors[ii][3]:
  if rank <> 0 then
     for jj from 1 to nops(coord) do
       print(nm + coord[jj] );
       ress:=simplify(expand(tt[nm][all][coord[jj]] - tt[nm][reg][coord[jj]]));
       print(expand(ress));
     end do:
  else
      print(nm);
      ress:=simplify(expand(tt[nm][all] - tt[nm][reg]));
      print(expand(ress));
  end if;

 end do;

end if;


# Definitions for Robot Dynamics Code Generation
# Init
# Erstelle Definitionen für die Maple-Skripte zur Berechnung von Kinematik und Dynamik des Roboters
#
# Quellen:
# [1] Sousa, C. D. and Cortesao, R.: Physical feasibility of robot base inertial parameter identification: A linear matrix inequality approach (2014)
# [2] Ayusawa, K. and Venture, G. and Nakamura, Y.: Identifiability and identification of inertial parameters using the underactuated base-link dynamics for legged multibody systems (2013)
# [3] Fujimoto, Y. and Obata, S. and Kawamura, A.: Robust biped walking with active interaction control between foot and ground (1998)
# [4] Khalil, W. and Kleinfinger, J.-F.: Minimum operations and minimum parameters of the dynamic models of tree structure robots (1987)
#
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-03
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
read "../helper/proc_MatlabExport":
read "../helper/proc_convert_t_s":
read "../helper/proc_vec2skew":
codegen_act := true:
# Lese Umgebungsvariable für Codegenerierung.
read "../robot_codegen_definitions/robot_env":
printf("Generiere Parameter für %s\n",robot_name):
# Robotics Definitions
# Joint Angles, Velocities and Accelerations in time depending and substitution form.
# Time Depending form is for time differentiation.
# Substitution Form is for partial differentiation and code export.
qJ_t := Matrix(NQJ, 1):
qJ_s := Matrix(NQJ, 1):
qJD_s := Matrix(NQJ, 1):
qJDD_s := Matrix(NQJ, 1):
for i from 1 to NQJ do
  qJ_t(i,1):=parse(sprintf("qJ%d(t)", i)):
  qJ_s(i,1):=parse(sprintf("qJ%ds", i)):
  qJD_s(i,1):=parse(sprintf("qJD%ds", i)):
  qJDD_s(i,1):=parse(sprintf("qJDD%ds", i)):
end do:
qJD_t := diff~(qJ_t, t):
qJDD_t := diff~(qJD_t, t):
# Gravity vector in world frame
if not assigned(g_world) then
  g_world := Matrix(3, 1):
  g_world(1 .. 3, 1) := <g1, g2, g3>:
end if:
# Position und Orientierung der Basis. Die Orientierung ist mit XYZ-Euler-Winkeln definiert, die aber nicht im weiteren Algorithmus verwendet werden (nur Platzhalter).
# Eine Invertierung der Orientierungsdarstellung sollte nicht notwendig werden, von daher kein Problem mit Orientierungsrepräsentationssingularität.
# gem. [2], S. 4 X_base_t SE(3): Position und Orientierung.
NQB := 6:
X_base_t:=Matrix(6, 1, [rx_base(t),ry_base(t),rz_base(t),alphax_base(t), betay_base(t), gammaz_base(t)]):
X_base_s:=Matrix(6, 1, [rxs_base,rys_base,rzs_base,alphaxs_base, betays_base, gammazs_base]):
# Geschwindigkeit der Basis (pelvis)
# Twist: Verallgemeinerte Geschwindigkeit. Diese Darstellung ist nicht konsistent (Ableitung der Position stimmt nicht mit Ausdruck der Geschwindigkeit überein)
V_base_t:=Matrix(6, 1, [diff~(rx_base(t),t),diff~(ry_base(t),t),diff~(rz_base(t),t),omegax_base(t), omegay_base(t), omegaz_base(t)]):
V_base_s:=Matrix(6, 1, [vxs_base,vys_base,vzs_base,omegaxs_base, omegays_base, omegazs_base]):
# Zeitableitung davon
VD_base_t:=diff~(V_base_t,t):
VD_base_s:=Matrix(6, 1, [vDxs_base,vDys_base,vDzs_base,omegaDxs_base, omegaDys_base, omegaDzs_base]):
# Name der Methode für die Orientierungsdarstellung der Basis
base_method_name := "twist":
# Umrechnung von der Ableitung der Basis-Orientierung zu Winkelgeschwindigkeiten:
# Wird hier nicht weiter beachtet (physikalisch falsch für eine Betrachtung der Dynamik der Basis).
# Ermöglicht eine einfachere Berechnung für Mechanismen, wo die Rückwirkung auf die Basis nicht in der Vorwärtsdynamik simuliert werden muss.
T_basevel := IdentityMatrix(3, 3):
# Verallgemeinerte Koordinaten, gem [2], S. 4, [3], S.1
NQ:=NQJ+NQB:
q_t := Matrix(NQ,1, <X_base_t, qJ_t>):
q_s := Matrix(NQ,1, <X_base_s, qJ_s>):
qD_t:= Matrix(NQ,1, <V_base_t, qJD_t>):
qD_s:= Matrix(NQ,1, <V_base_s, qJD_s>):
qDD_t:= Matrix(NQ,1, <VD_base_t, qJDD_t>):
qDD_s:= Matrix(NQ,1, <VD_base_s, qJDD_s>):
# MDH-Gelenkwinkel neu speichern (Definition der verallg. Koordinaten war dort noch nicht bekannt
theta := value(theta):
# Standard-Werte festlegen
# Anzahl der Körper (Number of Links):
if not assigned(NL) then
  NL := NJ + 1:
  printf("Variable NL ist nicht gegeben. Insgesamt %d Gelenke. Nehme an, dass jedes Gelenk einem Körper zugeordnet ist (keine Schleifen)\n", NJ):
else
  NVJ := NJ - (NL - 1):
  printf("Variable NL=%d ist gegeben. Insgesamt %d Gelenke. Davon sind die ersten %d einem Körper zugeordnet und die letzten %d virtuell.\n", NL, NJ, NJ-NVJ, NVJ):
end if:
# Gelenktyp (0=Revolute, 1=Prismatic, 2=Static). Sollte in der Definition festgelegt sein. Falls nicht, wird alles auf Revolute gesetzt
if not assigned(sigma) then
  sigma := Matrix(NJ,1):
  printf("Variable sigma ist nicht gegeben. Setze alle Gelenke als Drehgelenk (sigma=1)\n"):
end if:
# Aktuierung (1=aktiv, 0=passiv). Sollte in der Definition festgelegt sein. Falls nicht, wird alles auf Aktiv gesetzt
if not assigned(mu) then
  mu := Matrix(NJ,1,1):
  printf("Variable mu ist nicht gegeben. Setze alle Gelenke als aktiv (mu=1)\n"):
end if:
# Segmentreihenfolge der Roboterstruktur. Sollte in der Definition festgelegt sein. Falls nicht, wird eine serielle Kette angenommen
if not assigned(v) then
  v := Matrix(NL-1,1,i->i-1):
  printf("Variable v ist nicht gegeben. Setze serielle Kette (0,1,2,...)\n"):
end if:
# Parameter für Baumstruktur. Sollte in der Definition festgelegt sein. Falls nicht, wird eine serielle Kette angenommen
if not assigned(b) then
  b := Matrix(NJ,1):
  printf("Variable b ist nicht gegeben. zusätzliche Verschiebung auf Null\n"):
end if:
if not assigned(beta) then
  beta := Matrix(NJ,1):
  printf("Variable beta ist nicht gegeben. zusätzliche Verschiebung auf Null\n"):
end if:
# Eingabe prüfen
# Validität der Vorgänger-Indizes. Konvention: Vorgänger hat kleineren Index. Siehe Khalil-Publikationen (dort evtl nur implizit so verwendet).
for i from 1 to NJ do
  if v(i) >= i then
    printf("v(%d)=%d. Verletzt die Regel, dass die Vorgänger-Nummer kleiner sein muss als die aktuelle Nummer.\n", i, v(i)):
    quit: # Funktioniert in GUI nicht richtig...
    robot_name := "": # ...Daher auch Löschung des Roboternamens.
  end if:
end do:

# Dynamics Parameters
# Mass of each link
M_generic := Matrix(NL, 1):
for i from 1 to NL do
  M_generic[i,1]:=parse(sprintf("M%d", i-1)):
end do:
M := copy(M_generic):
if assigned(user_M) then
  if not RowDimension(user_M) = NL then
    printf("Input user_M is not of size %dx1. Error.\n", NL):
  end if:
  # Mass parameter given by user. Some entries can be zero or equal
  for i from 1 to NL do
    if user_M[i,1]<>M_generic[i,1] and user_M[i,1]<>0 then # check input
      printf("User input for mass %d should be either \"%s\" or zero. Not \"%s\". Continue anyway. \n", i-1, M_generic[i,1], user_M[i,1]):
    end if:
    M[i,1]:=user_M[i,1]:
  end do:
  printf("Masseparameter von Benutzer gesetzt. Eingabe für M:\n"):
  print(M);
end if:
# Center of Mass of each link (in link frame)
r_i_i_Si := Matrix(3, NL):
r_i_i_Si_generic := Matrix(3, NL):
for i from 1 to NL do
  r_i_i_Si_generic[1,i]:=parse(sprintf("SX%d", i-1)):
  r_i_i_Si_generic[2,i]:=parse(sprintf("SY%d", i-1)):
  r_i_i_Si_generic[3,i]:=parse(sprintf("SZ%d", i-1)):
end do:
if not assigned(user_CoM) then
  r_i_i_Si := r_i_i_Si_generic:
else
  if RowDimension(user_CoM) <> 3 or ColumnDimension(user_CoM) <> NL then
    printf("Input user_CoM is not of size 3x%d. Error.\n", NL):
  end if:
  for i from 1 to NL do
    for j from 1 to 3 do
      if user_CoM[j,i]<>r_i_i_Si_generic[j,i] and user_CoM[j,i]<>0 then # check input
        printf("User input for CoM %d,%d should be either \"%s\" or zero. Not \"%s\". Continue anyway. \n", j, i-1, r_i_i_Si_generic[j,i], convert(user_CoM[j,i],string)):
      end if:
    end do:
    r_i_i_Si[1..3,i]:=user_CoM[1..3,i]:
  end do:
  printf("Schwerpunktsparameter von Benutzer gesetzt. Eingabe für r_i_i_Si:\n"):
  print(r_i_i_Si);
end if:

# First Moment (mass and center of mass)
mr_i_i_Si := Matrix(3, NL):
mr_i_i_Si_generic := Matrix(3, NL):
xyzstrings := ["X", "Y", "Z"]:
for i from 1 to NL do
  for j from 1 to 3 do # loop over x-, y-, z-coordinates of the CoM
    mr_i_i_Si_generic[j,i]:= parse(sprintf("M%s%d", xyzstrings[j], i-1)):
  end do:
  if M[i,1] = 0 then next; end if: # Zero Mass can be set by the user. Then the first moment stays zero
  for j from 1 to 3 do # loop over x-, y-, z-coordinates of the CoM
    if r_i_i_Si[j,i] = parse(sprintf("S%s%d", xyzstrings[j], i-1)) then
    	 # default value for CoM is set (not overwritten by user input).
    	 # Set default value for first moment
      mr_i_i_Si[j,i]:= parse(sprintf("M%s%d", xyzstrings[j], i-1)):
    else
      # CoM is written by the user. Put this assumption also in the first moment to reduce dynamics parameters
    	 mr_i_i_Si[j,i] := M[i,1]*r_i_i_Si[j,i]:
    end if:
  end do:
end do:

# Inertia of each link (about the center of mass, in link frame)
I_i_Si := Matrix(6, NL):
I_i_Si_generic := Matrix(6, NL):
for i from 1 to NL do
  if M[i,1] = 0 then next; end if: # Zero Mass can be set by the user. Then the inertia stays zero
  I_i_Si_generic[1,i]:=parse(sprintf("XXC%d", i-1)):
  I_i_Si_generic[2,i]:=parse(sprintf("XYC%d", i-1)):
  I_i_Si_generic[3,i]:=parse(sprintf("XZC%d", i-1)):
  I_i_Si_generic[4,i]:=parse(sprintf("YYC%d", i-1)):
  I_i_Si_generic[5,i]:=parse(sprintf("YZC%d", i-1)):
  I_i_Si_generic[6,i]:=parse(sprintf("ZZC%d", i-1)):
end do:
if not assigned(user_inertia) then
  I_i_Si := I_i_Si_generic:
else
  if RowDimension(user_inertia) <> 6 or ColumnDimension(user_inertia) <> NL then
    printf("Input user_inertia is not of size 6x%d. Error.\n", NL):
  end if:
  for i from 1 to NL do
    for j from 1 to 6 do
      if user_inertia[j,i]<>I_i_Si_generic[j,i] and user_inertia[j,i]<>0 then # check input
        printf("User input for inertia %d,%d should be either \"%s\" or zero. Not \"%s\". Continue anyway. \n", j, i-1, convert(I_i_Si_generic[j,i],string), convert(user_inertia[j,i],string)):
      end if:
    end do:
    I_i_Si[1..6,i]:=user_inertia[1..6,i]:
  end do:
  printf("Trägheitsparameter von Benutzer gesetzt. Eingabe für I_i_Si:\n"):
  print(I_i_Si);
end if:

# Inertia of each link (about the origin of body frame, in link frame)
# Berechne die Inertial-Trägheitsmomente mit dem Steinerschen Verschiebungssatz
I_i_i_calc := Matrix(6, NL):
for i from 1 to NL do
  # Trägheitstensor (3x3) um den Körperschwerpunkt in Körper-KS
  I_i_Si_Tensor := Matrix([[I_i_Si[1, i], I_i_Si[2, i], I_i_Si[3, i]], [I_i_Si[2, i], I_i_Si[4, i], I_i_Si[5, i]], [I_i_Si[3, i], I_i_Si[5, i], I_i_Si[6, i]]]):
  # Steinerschen Satz anwenden
  I_i_i_Tensor := I_i_Si_Tensor + M[i,1]*Transpose(vec2skew(r_i_i_Si[1..3,i])) . vec2skew(r_i_i_Si[1..3,i]):
  # Wieder als Matrix abspeichern
  I_i_i_calc[..,i] := <I_i_i_Tensor[1,1]; I_i_i_Tensor[1,2]; I_i_i_Tensor[1,3]; I_i_i_Tensor[2,2]; I_i_i_Tensor[2,3]; I_i_i_Tensor[3,3]>:
end do:

# Allgemeine Form des Trägheitstensors (Einträge sind unabhängige Parameter)
I_i_i_generic := Matrix(6, NL):
for i from 1 to NL do
  I_i_i_generic[1,i]:=parse(sprintf("XX%d", i-1)):
  I_i_i_generic[2,i]:=parse(sprintf("XY%d", i-1)):
  I_i_i_generic[3,i]:=parse(sprintf("XZ%d", i-1)):
  I_i_i_generic[4,i]:=parse(sprintf("YY%d", i-1)):
  I_i_i_generic[5,i]:=parse(sprintf("YZ%d", i-1)):
  I_i_i_generic[6,i]:=parse(sprintf("ZZ%d", i-1)):
end do:

# Prüfe, welche Einträge des Trägheitstensors noch Schwerpunkts-Parameter enthalten. Diese müssen wieder auf die Standard-Werte mit unabhängigen Parametern gesetzt werden
I_i_i := Matrix(6, NL):
compstrings := ["XX", "XY", "XZ", "YY", "YZ", "ZZ"]:
for i from 1 to NL do
  if M[i,1] = 0 then next; end if: # Zero Mass can be set by the user. Then the inertia stays zero
  for j from 1 to 6 do # Alle Komponenten des Tensors
    # Nicht Null. Setze erstmal allgemeinen Eintrag:
    I_i_i[j,i] := I_i_i_generic[j,i]:
    # Prüfe, ob Schwerpunktsparameter vorkommt (durch Verschiebungssatz s.o.)
    IhasCoM := false:
    for k from 1 to 3 do
      if has(I_i_i_calc[j,i], r_i_i_Si_generic[k,i]) then
    	   IhasCoM := true: # Der Eintrag enthält die Schwerpunktskoordinaten. Nicht zulässig für weitere Rechnung.
    	   break:
      end if:
    end do:
    if has(I_i_i_calc[j,i], I_i_Si_generic[j,i]) then
    	 IhasCoM := true: # Es steht der Schwerpunktsbezogene Trägheitsterm drin. Weitere Rechnung damit nicht möglich.
    end if:
    if IhasCoM = true then
      next: # Es kommt ein Schwerpunktsparameter vor. Belasse Parameter auf allgemeinem Wert.
    end if:
    I_i_i[j,i] := I_i_i_calc[j,i]:
  end do:
end do:

# Vector of stacked dynamics parameters for regressor form
# Matrix of link inertial parameters, stacked link parameter vectors.
# Diese Parameter-Matrix wird nur benutzt, um für die Generierung der Regressorform danach abzuleiten.
# Hier stehen also die allgemeinen ("generic") Parameter drin. Es ist egal, ob diese bereits durch den Benutzer zu Null gesetzt sind.
# Dann würden die allgemeinen Parameter nicht in der Dynamik vorkommen und die Ableitung wäre Null.
PV2_mat := Matrix(NL, 10):
for i to NL do
  PV2_mat[i, 1 .. 6] := I_i_i_generic[1 .. 6, i]:
  PV2_mat[i, 7 .. 9] := mr_i_i_Si_generic[1 .. 3, i]:
  PV2_mat[i, 10] := M_generic[i, 1]:
end do:

# Parameter-Vektor Erstellen: vector of link inertial parameters (delta in [1]).
# Gleiche Überlegung wie für Parameter-Matrix
PV2_vec := Matrix(10*(NL), 1):
for i to NL do
  for j to 10 do
    PV2_vec[10*(i-1)+j] := PV2_mat[i, j]:
  end do:
end do:
# Kinematische Zwangsbedingungen
# Prüfe, ob kinematische Zwangsbedingungen in der Roboterkonfiguration genannt sind durch Prüfung der Existenz der entsprechenden Variablen.
if type( kintmp_t, 'Matrix') = false then
  kintmp_t := Matrix(1,1): # Dummy-Werte damit später alles funktioniert
  kintmp_s := Matrix(1,1):
end if:
# Export
save q_t, q_s, qD_t, qD_s, qDD_t, qDD_s, qJ_t, qJ_s, qJD_t, qJD_s, qJDD_t, qJDD_s, g_world, X_base_t, X_base_s, V_base_t, V_base_s, VD_base_t, VD_base_s, qoffset, theta, alpha, d, a,v,b,beta, sigma,mu,M, r_i_i_Si, mr_i_i_Si, I_i_i, I_i_Si, PV2_vec, PV2_mat, robot_name, NQ,NQB,NQJ,NJ,NL, base_method_name, T_basevel, kintmp_t, kintmp_s, sprintf("../codeexport/%s/tmp/tree_floatb_twist_definitions", robot_name):
save q_t, q_s, qD_t, qD_s, qDD_t, qDD_s, qJ_t, qJ_s, qJD_t, qJD_s, qJDD_t, qJDD_s, g_world, X_base_t, X_base_s, V_base_t, V_base_s, VD_base_t, VD_base_s, qoffset, theta, alpha, d, a,v,b,beta, sigma,mu,M, r_i_i_Si, mr_i_i_Si, I_i_i, I_i_Si, PV2_vec, PV2_mat, robot_name, NQ,NQB,NQJ,NJ,NL, base_method_name, T_basevel, kintmp_t, kintmp_s, sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
# Einzelne DH-Parameter als Matlab-Code exportieren. Damit lässt sich in Matlab ein passender Parametersatz generieren.
# Die folgenden Dateien müssen immer generiert werden, wenn die Bash-Skripte zur Funktionszusammensetzung fehlerfrei laufen sollen.
# Benutze die Funktion convert_t_s, um eventuelle Substitutionsvariablen für konstante Gelenkwinkel zu verwenden, da die Matlab-Terme auch mit substituierten Ausdrücken generiert werden.
# Zur Kennzeichnung von zeitabhängigen und konstanten Ausdrücken kann "delta1(t)", "delta1" und "delta1s" verwendet werden.
MatlabExport(v, sprintf("../codeexport/%s/tmp/parameters_mdh_v_matlab.m", robot_name), 2):
MatlabExport(convert_t_s(a), sprintf("../codeexport/%s/tmp/parameters_mdh_a_matlab.m", robot_name), 2):
d_export := d *~ (1-~sigma):
MatlabExport(convert_t_s(d_export), sprintf("../codeexport/%s/tmp/parameters_mdh_d_matlab.m", robot_name), 2):
theta_export := theta *~ sigma:
MatlabExport(convert_t_s(theta_export), sprintf("../codeexport/%s/tmp/parameters_mdh_theta_matlab.m", robot_name), 2):
MatlabExport(convert_t_s(b), sprintf("../codeexport/%s/tmp/parameters_mdh_b_matlab.m", robot_name), 2):
MatlabExport(convert_t_s(alpha), sprintf("../codeexport/%s/tmp/parameters_mdh_alpha_matlab.m", robot_name), 2):
MatlabExport(convert_t_s(beta), sprintf("../codeexport/%s/tmp/parameters_mdh_beta_matlab.m", robot_name), 2):
MatlabExport(convert_t_s(qoffset), sprintf("../codeexport/%s/tmp/parameters_mdh_qoffset_matlab.m", robot_name), 2):
MatlabExport(sigma, sprintf("../codeexport/%s/tmp/parameters_mdh_sigma_matlab.m", robot_name), 2):
MatlabExport(mu, sprintf("../codeexport/%s/tmp/parameters_mdh_mu_matlab.m", robot_name), 2):

# Einzelne Dynamikparameter als Matlab-Code exportieren. Wenn die Parameter durch Benutzereingaben verändert wurden, lässt sich diese Information so weiter benutzen.
# (z.B. in der Definition von Eingabeparametern in den Testskripten).
# In den Maple-Variablen ist die Spalte der Index der Körper und die Zeilen sind die Indizes für die xyz-Komponenten (einfacherer Aufruf der Variablen)
# In Matlab ist es umgekehrt (führt zu konsistenterem Code in Matlab): Die Zeilen entsprechen dem Körper-Index. Daher hier Transponierung.
# Zusätzlich ist die Reihenfolge der Komponenten der Trägheitstensoren in Matlab und Maple unterschiedlich (daher die Indizierung).
# Matlab: xx, yy, zz, xy, xz, yz (erst Hauptmomente, dann Deviationsmomente)
# Maple: xx, xy, xz, yy, yz, zz (Dreiecksform)
MatlabExport(M, sprintf("../codeexport/%s/tmp/parameters_dyn_mges_matlab.m", robot_name), 2):
MatlabExport(Transpose(r_i_i_Si), sprintf("../codeexport/%s/tmp/parameters_dyn_rSges_matlab.m", robot_name), 2):
MatlabExport(Transpose(I_i_Si([1,4,6,2,3,5],..)), sprintf("../codeexport/%s/tmp/parameters_dyn_Icges_matlab.m", robot_name), 2):
MatlabExport(Transpose(mr_i_i_Si), sprintf("../codeexport/%s/tmp/parameters_dyn_mrSges_matlab.m", robot_name), 2):
MatlabExport(Transpose(I_i_i([1,4,6,2,3,5],..)), sprintf("../codeexport/%s/tmp/parameters_dyn_Ifges_matlab.m", robot_name), 2):


input := FileTools:-Text:-ReadFile("AoC-2021-6-input.txt" ):
tally := table(sparse=0,Statistics:-Tally(
    StringTools:-Split(input,",")));
lanternfish := Array(0..8, [tally["0"],tally["1"],tally["2"],tally["3"],
    tally["4"],tally["5"],0,0,0]);

to 256 do
    day0 := lanternfish[0];
    lanternfish[0..7] := lanternfish[1..8];
    lanternfish[8] := day0;
    lanternfish[6] := lanternfish[6] + day0;
end do:

answer2 := add(lanternfish);


(*
 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_c_vsxc_params *params;

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

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

$include "gvt4.mpl"

vsxc_comp := (rs, z, spin, xs, ts) ->
  + lda_stoll_par(f_pw, rs,  z,  1)
  * gtv4(params_a_alpha_ss, params_a_dss, xs, 2*(ts - K_FACTOR_C))
  * Fermi_D(xs, ts):

(* The parallel and perpendicular components of the energy *)
vsxc_fpar  := (rs, z, xs0, xs1, ts0, ts1) ->
  + vsxc_comp(rs,  z,  1, xs0, ts0)
  + vsxc_comp(rs, -z, -1, xs1, ts1):

vsxc_fperp := (rs, z, xs0, xs1, ts0, ts1) ->
  + lda_stoll_perp(f_pw, rs,  z)
  * gtv4(params_a_alpha_ab, params_a_dab, sqrt(xs0^2 + xs1^2), 2*(ts0 + ts1 - 2*K_FACTOR_C)):

vsxc_f := (rs, z, xs0, xs1, ts0, ts1) ->
  + vsxc_fpar (rs, z, xs0, xs1, ts0, ts1)
  + vsxc_fperp(rs, z, xs0, xs1, ts0, ts1):

f := (rs, z, xt, xs0, xs1, us0, us1, ts0, ts1) ->
  vsxc_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_c *)

$include "gga_c_pw91.mpl"

c1 := 1.1015:
c2 := 0.6625:

f  := (rs, z, xt, xs0, xs1) ->
  + c1*f_pw91(rs, z, xt, xs0, xs1) + (c2 - c1)*(
  +    f_pw91(rs*(2*(1 + z))^(1/3),  1, xs0, xs0, 0)
  +    f_pw91(rs*(2*(1 - z))^(1/3), -1, xs1, 0, 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 *)

$include "mgga_c_scan.mpl"

(* we need to redefine two functions *)
scan_e0_g := (rs, z, t) -> (1 + 8*A(rs, z, t)*t^2)^(-1/4)/2
       + (1 + 80*A(rs, z, t)^2*t^4)^(-1/8)/2:
scan_g_infty := s -> (1 + 4*scan_chi_infty*s^2)^(-1/4)/2
       + (1 + 80*scan_chi_infty^2*s^4)^(1/8)/2:

(* and the new parameters *)
scan_b1c :=  0.030197:
scan_b2c := -0.06623:
scan_b3c :=  0.16672:

(* set parameters of f_alpha *)
params_a_c1 := 1.131:
params_a_c2 := 1.7:
params_a_d  := 1.37:


make_GL3_2 := proc()
 global GL3_2;
 local G,L,E,f,n,i,j,k,g,h;

 G := table():

 L := [[]]:
 for i from 1 to 9 do
  L := [seq(seq([op(u),j],j=0..1),u in L)];
 od:

 # Strange order ensures that 1 comes first, followed by other
 # upper unitriangular matrices.
 f := (u) -> Matrix([[u[7],u[5],u[6]],
                     [u[1],u[8],u[4]],
		     [u[3],u[2],u[9]]]);
 L := select(u -> mods(Determinant(f(u)),2) <> 0,L);
 n := nops(L);
 E := map(f,L);

 G["elements"] := E;

 G["index"] := table():
 for i from 1 to n do G["index"][convert(E[i],listlist)] := i; od:

 G["id"] := IdentityMatrix(3);
 G["o"] := (u,v) -> map(mods,u . v,2);
 G["inv"] := (u) -> map(mods,1 / u,2);

 G["o_table"] := table();
 G["inv_table"] := table();

 for i from 1 to n do
  g := G["elements"][i];
  G["inv_table"][i] := G["index"][convert(G["inv"](g),listlist)];

  for j from 1 to n do
   h := G["elements"][j];
   G["o_table"][i,j] := G["index"][convert(G["o"](g,h),listlist)];
  od:
 od:

 G["ee"] := convert(IdentityMatrix(n),listlist);

 G["ring_id"] := G["ee"][1];

 G["antipode"] := proc(u)
  local n,M,w,i,j;
  n := 168;
  M := eval(GL3_2["inv_table"]);
  w := [seq(u[M[i]],i=1..n)];
  return w;
 end:

 G["convolve"] := proc(u,v)
  local n,J,M,w,i,j,ii;
  n := 168;
  J := eval(GL3_2["inv_table"]);
  M := eval(GL3_2["o_table"]);
  w := [0$n];
  for i from 1 to n do
    if u[i] <> 0 then
    ii := J[i];
    w := w +~ [seq(u[i] * v[M[ii,j]],j=1..n)];
   fi;
  od;

  return w;
 end:

 G["borel_elements"] :=
   [seq(seq(seq(Matrix([[1,i,j],[0,1,k],[0,0,1]]),i=0..1),j=0..1),k=0..1)]:
 G["borel_indices"] :=
   map(b -> G["index"][convert(b,listlist)],G["borel_elements"]);
 G["borel_sum"] := [0$168]:
 for i in G["borel_indices"] do
  G["borel_sum"] := G["borel_sum"] +~ G["ee"][i]:
 od:

 G["coxeter_elements"] := map(permutation_matrix,combinat[permute](3)):
 G["coxeter_indices"] := map(w -> G["index"][convert(w,listlist)],G["coxeter_elements"]):
 G["signed_coxeter_sum"] := [0$168]:
 for i in G["coxeter_indices"] do
  G["signed_coxeter_sum"] := G["signed_coxeter_sum"] +~ Determinant(E[i]) *~ G["ee"][i]:
 od:

 G["steinberg_idempotent"] :=
  G["convolve"](G["signed_coxeter_sum"], G["borel_sum"]) / 21;

 GL3_2 := eval(G);
 return(eval(G));
end:


# Lagrange Formalis for Robot based on MDH frames
# Einleitung
# Berechnung des Lagrange-Formalismus
#
# 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
# lagrange -> Berechnung des Lagrange-Formalismus
# worldframe -> Berechnung basierend auf Energien aus Welt-KS (KS W)
# par12 -> Parametersatz 1 (Schwerpunkt als Parameter: SX,SY,SZ) oder Parametersatz 2 (1. und 2. Moment MX,MY,MZ,...)
# Autor
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-03
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Sources
# [GautierKhalil1990] Direct Calculation of Minimum Set of Inertial Parameters of Serial Robots
# [KhalilDombre2002] Modeling, Identification and Control of Robots
# [Ortmaier2014] Vorlesungsskript Robotik I
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(ArrayTools):
with(codegen):
with(CodeGeneration):
with(StringTools):
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
codegen_opt := 2:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../helper/proc_Lagrange1":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotx":
read "../transformation/proc_troty":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../transformation/proc_trafo_mdh":
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
read sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
kin_constraints_exist := kin_constraints_exist: # nur zum Abschätzen der Komplexität
;
# Einstellungen für Term-Vereinfachungen: 0=keine, 1=dTdq_s, 2=auch dTdq_s, 3=auch dTdqDdt_s
if not assigned(simplify_options) or simplify_options(8)=-1 then # Standard-Einstellungen:
  # 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.
  # Annahme: Bei kinematischen Zwangsbedingungen werden die Terme zu kompliziert.
  if not kin_constraints_exist then # normale serielle Ketten und Baumstrukturen
    use_simplify := 0: # Standardmäßig aus
  else # mit kinematischen Zwangsbedingungen
    use_simplify := 1: # Annahme, dass Optimierung der Gravitations-Terme noch möglich ist
  end if:
else # Benutzer-Einstellungen:
  use_simplify := simplify_options(8): # achter Eintrag ist für Lagrange
end if:

# Kennung des Parametersatzes, für den die Dynamikfunktionen erstellt werden sollen. Muss im Repo und in der mpl-Datei auf 1 gelassen werden, da die folgende Zeile mit einem Skript verarbeitet wird.
codegen_dynpar := 1:
# Ergebnisse der Energie laden
if codegen_dynpar = 1 then
  read sprintf("../codeexport/%s/tmp/energy_potential_floatb_%s_worldframe_par1_maple.m", robot_name, base_method_name):
  read sprintf("../codeexport/%s/tmp/energy_kinetic_floatb_%s_worldframe_par1_maple.m", robot_name, base_method_name):
elif codegen_dynpar = 2 then
  read sprintf("../codeexport/%s/tmp/energy_potential_floatb_%s_worldframe_par2_maple.m", robot_name, base_method_name):
  read sprintf("../codeexport/%s/tmp/energy_kinetic_floatb_%s_linkframe_par2_maple.m", robot_name, base_method_name):
else
  printf("Energiefunktionen nur für Parametersatz 1 oder 2 definiert\n"):
end:
T := T:
U_grav := U_grav:
printf("%s. Generiere Terme des Lagrange-Formalismus für %s mit Parametersatz %d und %s\n", \
  FormatTime("%Y-%m-%d %H:%M:%S"), robot_name, codegen_dynpar, base_method_name):

# Lagrange Formalismus (mit Funktion)
tmp_t1:=time():
OutputLagrange := Lagrange1(T, U_grav, NQ):
dTdqDdt_s := OutputLagrange[1]:
dTdq_s := OutputLagrange[2]:
dUdq_s := OutputLagrange[3]:
printf("%s. Lagrange-Formalismus für %s mit Parametersatz %d und %s berechnet. Rechenzeit %1.1fs.\n", \
  FormatTime("%Y-%m-%d %H:%M:%S"), robot_name, codegen_dynpar, base_method_name, time()-tmp_t1):
# Terme vereinfachen: Einzeln und komponentenweise
if use_simplify>=1 then
  # Term dUdq_s
  tmp_t1:=time(): tmp_l1 := length(dUdq_s):
  printf("%s. Beginne Vereinfachung: Lagrange-Term dU/dq (Param. %d). Länge: %d.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l1):
  for i from 1 to NQ do
    dUdq_s[i,1] := simplify2(dUdq_s[i,1]):
  end do:
  tmp_t2:=time(): tmp_l2 := length(dUdq_s):
  printf("%s. Lagrange-Term dU/dq (Param. %d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
end if:
if use_simplify>=2 then # Die Terme können sehr kompliziert sein. Daher nicht immer vereinfachen.
  # Term dTdq_s
  for i from 1 to NQ do
    tmp_t1:=time(): tmp_l1 := length(dTdq_s[i,1]):
    printf("%s. Beginne mit Vereinfachung von Lagrange-Term dT/dq (Param. %d; Komponente %d/%d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, NQ, tmp_l1):
    dTdq_s[i,1] := simplify2(dTdq_s[i,1]):
    tmp_t2:=time(): tmp_l2 := length(dTdq_s[i,1]):
    printf("%s. Lagrange-Term dT/dq (Param. %d; Komponente %d/%d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, NQ, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end do:
end if:
if use_simplify>=3 then
  # Term dTdqDdt_s
  for i from 1 to NQ do
    tmp_t1:=time(): tmp_l1 := length(dTdqDdt_s[i,1]):
    printf("%s. Beginne mit Vereinfachung von Lagrange-Term d(dT/dqD)/dt (Param. %d; Komponente %d/%d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, NQ, tmp_l1):
    dTdqDdt_s[i,1] := simplify2(dTdqDdt_s[i,1]):
    tmp_t2:=time(): tmp_l2 := length(dTdqDdt_s[i,1]):
    printf("%s. Lagrange-Term d(dT/dqD)/dt (Param. %d; Komponente %d/%d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, NQ, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end do:
end if:
save dUdq_s, sprintf("../codeexport/%s/tmp/floatb_%s_lagrange_dUdq_s_par%d_maple.m", robot_name, base_method_name, codegen_dynpar):
save dTdq_s, sprintf("../codeexport/%s/tmp/floatb_%s_lagrange_dTdq_s_par%d_maple.m", robot_name, base_method_name, codegen_dynpar):
save dTdqDdt_s, sprintf("../codeexport/%s/tmp/floatb_%s_lagrange_dTdqDdt_s_par%d_maple.m", robot_name, base_method_name, codegen_dynpar):
printf("%s. Lagrange-Formalismus durchgeführt und Ergebnisse gespeichert (Dynamik-Parametersatz %d)\n", \
  FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar):


`phi/F1bar/ord` := (A::set) -> proc(x)
 return sort([op(A)],(a,b) -> ((a=b) or (op(x[{a,b}][a]) < op(x[{a,b}][b]))));
end:

######################################################################
# Apply the isomorphism R^1 = R

`trim/F1bar` := (A::set) -> proc(x)
 local u,x0,P,T,a;

 x0 := table();
 P := `list_elements/big_subsets`(A);
 for T in P do
  u := `bottom_normalise/SW`(1)(T)(x[T]);
  x0[T] := table();
  for a in T do
   x0[T][a] := op(u[a]);
  od;
 od;

 return eval(x0):
end:

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

`subcritical_tree/F1bar` := (A::set) -> proc(x)
 local x0,R,n,SS,i,j,J,J1,g1,g2,is_subcritical,a,k;

 x0 := `trim/F1bar`(A)(x);

 R := `phi/F1bar/ord`(A)(x);
 n := nops(A);
 SS := seq({a},a in A);

 for i from 1 to n-1 do
  for j from i+1 to n do
   J := {seq(R[k],k=i..j)};
   is_subcritical := true;
   if i > 1 then
    J1 := {R[i-1],op(J)};
    g1 := `gap/real_functions`(J )(x0[J1]);
    g2 := `gap/real_functions`(J1)(x0[J1]);
    if g2 = g1 then
     is_subcritical := false;
    fi;
   fi;
   if j < n then
    J1 := {op(J),R[j+1]};
    g1 := `gap/real_functions`(J )(x0[J1]);
    g2 := `gap/real_functions`(J1)(x0[J1]);
    if g2 = g1 then
     is_subcritical := false;
    fi;
   fi;

   if is_subcritical then
    SS := SS,J;
   fi;
  od;
 od;
 SS := {SS};
 return SS;
end:

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

`theta/F1bar/K` := (A::set) -> proc(x)
 local x0,R,n,SS,i,j,k,J,J1,t,parent;

 x0 := `trim/F1bar`(A)(x);
 R := `phi/F1bar/ord`(A)(x);
 SS := `subcritical_tree/F1bar`(A)(x);
 parent := parent_map(A)(SS);
 n := nops(A);
 t := table();

 for i from 1 to n do
  for j from i to n do
   J := {seq(R[k],k=i..j)};
   if nops(J) = 1 or nops(J) = n then
    t[J] := 1;
   elif not(member(J,SS)) then
    t[J] := 0;
   else
    J1 := parent[J];
    t[J] := 1 - `gap/real_functions`(J)(x0[J1])/
                `gap/real_functions`(J1)(x0[J1]);
   fi;
  od;
 od;

 return [R,eval(t)];
end:

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

`phi/K/F1bar` := (A::set) -> proc(Rt)
 local R,t,n,r,i,j,k,l,x,P,Q,Q1,J,M,M1,m,T,U;

 R,t := op(Rt);

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

 x := table();
 m := table();

 P := `list_elements/big_subsets`(A);
 Q := {seq(seq([seq(R[k],k=i..j)],j=i..n),i=1..n)};
 Q1 := select(J -> nops(J)>1,Q);

 for J in Q do
  m[J] := table();
  M := select(U -> ({op(J)} minus {op(U)} <> {}),Q);
  for i from 1 to nops(J) - 1 do
   M1 := select(U -> member(J[i],U) and member(J[i+1],U),M);
   m[J][J[i]] := mul(1-t[{op(U)}],U in M1);
  od;
 od;

 for T in P do
  x[T] := table();
  i := min(op(map(a -> r[a],T)));
  j := max(op(map(a -> r[a],T)));
  J := [seq(R[k],k=i..j)];
  for k from i to j do
   if member(R[k],T) then
    x[T][R[k]] := [add(m[J][R[l]],l=i..k-1)];
   fi
  od;
 od;

 return eval(x);
end:


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

`eta/height_functions`:= (A::set) -> `if`(nops(A)=1,table(A=0),FAIL);

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

`gamma/height_functions`:= (A::set,B::set) -> (p) -> proc(j,h)
 local k,P,phi,b0,b1,m,U,pU,b;

 k := table();
 P := `list_elements/nonempty_subsets`(A);

 phi := table();
 for b0 in B do
  m := 1;
  for b1 in B do
   if b1 <> b0 then m := min(m,j[{b0,b1}]); fi;
  od;
  phi[b0] := m;
 od;

 for U in P do
  pU := map(u -> p[u],U);
  if nops(pU) > 1 then
   k[U] := j[pU];
  else
   b := op(pU);
   k[U] := phi[b] * h[b][U];
  fi;
 od;

 return eval(k);
end;

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

`gamma_min/height_functions`:= (A::set,B::set) -> (p) -> proc(j,h)
 local k,P,phi,b0,b1,m,U,pU,b;

 k := table();
 P := `list_elements/nonempty_subsets`(A);

 phi := table();
 for b0 in B do
  m := 1;
  for b1 in B do
   if b1 <> b0 then m := min(m,j[{b0,b1}]); fi;
  od;
  phi[b0] := m;
 od;

 for U in P do
  pU := map(u -> p[u],U);
  if nops(pU) > 1 then
   k[U] := j[pU];
  else
   b := op(pU);
   k[U] := min(phi[b] , h[b][U]);
  fi;
 od;

 return eval(k);
end;


read "../IdentifiabilityODE.mpl";

sys := [
diff(EGF_EGFR(t), t) = reaction_1_k1*EGF_EGFR(t) - reaction_9_k1*EGF_EGFR(t) - reaction_1_k2*EGF_EGFR(t),
diff(EGFR(t), t) = -reaction_1_k1*EGF_EGFR(t) + reaction_1_k2*EGF_EGFR(t) - EGFR(t)*EGFR_turnover + EGFR_turnover*pro_EGFR(t),
diff(pEGFR(t), t) = -reaction_4_k1*pEGFR(t) + reaction_9_k1*EGF_EGFR(t) - pEGFR(t)*Akt(t)*reaction_2_k1 + reaction_3_k1*pEGFR_Akt(t) + pEGFR_Akt(t)*reaction_2_k2,
diff(pAkt_S6(t), t) = pAkt(t)*reaction_5_k1*S6(t) - reaction_6_k1*pAkt_S6(t) - pAkt_S6(t)*reaction_5_k2,
diff(pEGFR_Akt(t), t) = pEGFR(t)*Akt(t)*reaction_2_k1 - reaction_3_k1*pEGFR_Akt(t) - pEGFR_Akt(t)*reaction_2_k2,
diff(pAkt(t), t) = -pAkt(t)*reaction_7_k1 - pAkt(t)*reaction_5_k1*S6(t) + reaction_6_k1*pAkt_S6(t) + reaction_3_k1*pEGFR_Akt(t) + pAkt_S6(t)*reaction_5_k2,
diff(S6(t), t) = pS6(t)*reaction_8_k1 - pAkt(t)*reaction_5_k1*S6(t) + pAkt_S6(t)*reaction_5_k2,
diff(pS6(t), t) = -pS6(t)*reaction_8_k1 + reaction_6_k1*pAkt_S6(t),
diff(Akt(t), t) = pAkt(t)*reaction_7_k1 - pEGFR(t)*Akt(t)*reaction_2_k1 + pEGFR_Akt(t)*reaction_2_k2,
y1(t) = pEGFR(t)*a1 + a1*pEGFR_Akt(t),
y2(t) = a2*pAkt(t) + a2*pAkt_S6(t),
y3(t) = pS6(t)*a3
];
CodeTools[CPUTime](IdentifiabilityODE(sys, GetParameters(sys)));


(*
 Copyright (C) 2020 Susi Lehtola

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

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

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

(* Equation (20) *)
lgap_f0 := s -> 1 + params_a_kappa*(1-exp(-add(params_a_mu[i]*s^(i), i=1..3))):
lgap_f := x -> lgap_f0(X2S*x):

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


(*
 Copyright (C) 2020 Susi Lehtola

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

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

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

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

(* These come from pbe correlation *)
params_a_gamma := (1 - log(2))/Pi^2:
mgamma := params_a_gamma:

(* r2scan values *)
params_a_dp2 := 0.361:

(* Equation (S6) *)
r2scan_alpha := (z, xt, ts0, ts1) -> (t_total(z, ts0, ts1) - xt^2/8) / (K_FACTOR_C*t_total(z, 1, 1) + params_a_eta*xt^2/8):

(* Equation (S26) *)
r2scan_f_alpha_neg := a -> exp(-params_a_c1*a/(1 - a)):
r2scan_f_alpha := (a, ff) -> my_piecewise5(
  a <= 0, r2scan_f_alpha_neg(m_min(a, 0)),
  a <= 2.5, rscan_f_alpha_small(m_min(a, 2.5), ff),
  rscan_f_alpha_large(m_max(a, 2.5))):

(* Equation (S28) *)
r2scan_d := z -> (opz_pow_n(z,5/3) + opz_pow_n(-z,5/3))/2:

(* Equation (S33): beta(rs), this is the same as in gga_c_regtpss *)
beta_a := 0.066724550603149220:
beta_b := 0.1:
beta_c := 0.1778:
mbeta := (rs) -> beta_a*(1 + beta_b*rs)/(1 + beta_c*rs):

(* Equation (S30) *)
w1 := (rs, z) -> exp(-f_pw(rs, z)/(mgamma*mphi(z)^3)) - 1:

(* Equation (S27); note that the paper indexes starting from zero *)
r2scan_dfc2 := ff -> add(i*ff[8-i], i=1..7):

(* According to James Furness, this is LSDA0 - see also Equation (S25) *)
r2scan_elsda0 := (rs, z) -> scan_eclda0(rs)*scan_Gc(z):
(* while LSDA1 is just Perdew-Wang *)
r2scan_elsda1 := (rs, z) -> f_pw(rs, z):

(* Derivatives wrt rs *)
r2scan_delsda0 := (rs, z) -> eval(diff(r2scan_elsda0(x1, x2), x1), [x1=rs, x2=z]):
r2scan_delsda1 := (rs, z) -> eval(diff(r2scan_elsda1(x1, x2), x1), [x1=rs, x2=z]):

(* Equation (S34) *)
r2scan_dy := (rs, z, s) -> r2scan_dfc2(rscan_fc)/(27 * mgamma * r2scan_d(z) * mphi(z)^3 * w1(rs, z)) * (
    + 20*rs*(r2scan_delsda0(rs, z) - r2scan_delsda1(rs, z))
    - 45*params_a_eta*(r2scan_elsda0(rs, z) - r2scan_elsda1(rs, z))
  ) * s^2*exp(-s^4/params_a_dp2^4):

(* Equation (S32) *)
r2scan_y := (rs, z, t) -> mbeta(rs)*t^2/(mgamma*w1(rs, z)):

(* Equation (S31) *)
r2scan_g := (rs, z, s, t) -> 1/(1 + 4*(r2scan_y(rs, z, t) - r2scan_dy(rs, z, s)))^(1/4):

(* Equation (S29) *)
fH := (rs, z, s, t) -> mgamma*mphi(z)^3*log(1 + w1(rs, z)*(1 - r2scan_g(rs, z, s, t))):

(* Now we can build ec1 from (S24) *)
r2scan_ec1 := (rs, z, s, t) -> f_pw(rs, z) + fH(rs, z, s, t):

(* Equation (S35)-(S41) are same as SCAN *)
r2scan_ec0 := (rs, z, s) -> scan_e0(rs, z, s):

(* and the functional itself *)
r2scan_f := (rs, z, xt, xs0, xs1, ts0, ts1) ->
  r2scan_ec1(rs, z, X2S*2^(1/3)*xt, tt(rs, z, xt)) + r2scan_f_alpha(r2scan_alpha(z, xt, ts0, ts1), rscan_fc)*(
    + r2scan_ec0(rs, z, X2S*2^(1/3)*xt) - r2scan_ec1(rs, z, X2S*2^(1/3)*xt, tt(rs, z, xt))):

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


######################################################################
# trees(A) is the set of trees on A
# (Section sec-trees)

`is_element/trees` := (A::set) -> proc(TT)
 local n,i,j,T,U,is_min,M;
 global reason;

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

 n := nops(TT);
 is_min := table;
 for i from 1 to n do
  is_min[i] := true;
  T := TT[i];
  if not type(T,set) and T <> {} and T minus A = {} then
   reason := [convert(procname,string),"T is not a nonempty subset of A",T,A];
   return false;
  fi;
 od;

 # Now check nesting, and along the way, check which sets are minimal.
 for i from 1 to n do
  T := TT[i];
  for j from 1 to i-1 do
   U := TT[j];
   if T intersect U <> {} then
    if T minus U = {} then
     is_min[j] := false;
    else
     if U minus T = {} then
      is_min[i] := false;
     else
      reason := [convert(procname,string),"T and U are neither disjoint nor nested",T,U];
      return false;
     fi;
    fi;
   fi;
  od;
 od;

 # Check that the union of the minimal sets is A.
 M := NULL;
 for i from 1 to n do
  if is_min[i] then M := M,op(TT[i]); fi;
 od;
 if A minus {M} <> {} then
  reason := [convert(procname,string),"The minimal sets do not cover A",M,A];
  return false;
 fi;

 return true;
end;

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

`is_equal/trees` := (A::set) -> (TT,UU) -> evalb(TT = UU);

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

`is_leq/trees` := (A::set) -> (TT,UU) -> evalb(TT minus UU = {});

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

`is_separated/trees` := (A::set) -> proc(TT)
 local a;
 return evalb({seq({a},a in A)} minus TT = {});
end:

######################################################################
# Suppose we have a set A, a tree TT on A, a set T in TT, and a point a that
# does not lie in A.  We can then construct a new tree on A u {a} by attaching
# a to the node T.  There are two slightly different ways to do this.
# We can either attach a to a new node just above T, or we can attach a to
# the node T itself.  (However, the second version is invalid if |T|=1).

`attach_to_tree` := (A::set) -> proc(TT,T,a,above_)
 local above,CT,SCT,NCT;

 above := `if`(nargs > 3, above_, false);

 if not(`is_element/trees`(A)(TT) and
        member(T,TT) and
        not(member(a,A))) then
  return FAIL;
 fi;

 if nops(T)=1 and not(above) then
  return FAIL;
 fi;

 # Sets that contain (or are equal to) T
 CT := select(U -> (T minus U = {}),TT);

 SCT := CT minus {T};

 # Sets that do not contain T
 NCT := TT minus CT;

 if above then
  return {{a},op(NCT),T,op(map(U -> U union {a},CT))};
 else
  return {{a},op(NCT),op(map(U -> U union {a},CT))};
 fi;
end;

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

`random_element/trees`:= (A::set) -> proc()
 local n,a,A1,T0,T1,T,above;

 # Degenerate cases for small A
 if A={} then return {}; fi;
 if nops(A)=1 then return {A}; fi;

 # If A is not too small, we write it as {a} union A1.  Then we recursively
 # choose a random tree on A1, and attach a to it.

 n := nops(A);
 a := A[rand(1..n)()];
 A1 := A minus {a};
 if n = 2 then
  above := true;
 else
  above := evalb(rand(0..1)() = 1);
 fi;

 T0 := `random_element/trees`(A1)();
 T1 := select(U -> nops(U) > 1,T0);

 if above then
  T := T0[rand(1..nops(T0))()];
 else
  T := T1[rand(1..nops(T1))()];
 fi;

 T := `attach_to_tree`(A1)(T0,T,a,above);

 return T;
end;

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

`big_sets/trees` := (TT) -> select(T -> nops(T) > 1,TT);

######################################################################
# Given a tree TT, this returns a table P indexed by the non-maximal
# elements of TT, such that P[T] is the parent of T in TT.

`parent_map` := (A::set) -> proc(TT)
 local TS,n,i,j,T,U,parent;

 # if not(`is_element/trees`(A)(TT)) then return FAIL; fi;

 TS := sort([op(TT)],(a,b) -> (nops(a) < nops(b)));
 n := nops(TS);
 parent := table();

 for i from 1 to n do
  T := TS[i];
  parent[T] := FAIL;
  for j from i+1 to n do
   U := TS[j];
   if T minus U = {} then
    parent[T] := U;
    break;
   fi;
  od;
 od;

 return eval(parent);
end;

######################################################################
# Given a tree TT, this returns a table C indexed by the
# elements of TT, such that C[T] is the set of children of T in
# TT.  In particular, C[T] = {} if T is minimal.

`children_map` := (A::set) -> proc(TT)
 local parent,children,TT0,T,U;

 parent := eval(`parent_map`(A)(TT));

 children := table();
 for T in TT do
  children[T] := {};
 od:

 TT0 := TT minus {A};

 for T in TT0 do
  U := parent[T];
  if U <> FAIL then
   children[U] := {op(children[U]),T};
  fi;
 od;

 return eval(children);
end;

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

corolla := proc(A::set)
 local a;
 return {A,seq({a},a in A)};
end: