text:
##################################################################
"""
$(SIGNATURES)

Extract value from dual number. Use to debug physics callbacks.
Re-exported from ForwardDiff.jl
"""
const value=ForwardDiff.value
const partials=ForwardDiff.partials
const npartials=ForwardDiff.npartials

# Add value to matrix if it is nonzero
@inline function _addnz(matrix,i,j,v::Tv,fac) where Tv
    if isnan(v)
        error("trying to assemble NaN")
    end
    if v!=zero(Tv)
        updateindex!(matrix,+,v*fac,i,j)
    end
end

"""
$(TYPEDEF)

Exception thrown if Newton's method convergence fails.
"""
struct ConvergenceError <: Exception
end

"""
$(TYPEDEF)

Exception thrown if error occured during assembly (e.g. domain error)
"""
struct AssemblyError <: Exception
end

"""
$(TYPEDEF)

Exception thrown if error occured during factorization.
"""
struct FactorizationError <: Exception
end

"""
$(TYPEDEF)

Exception thrown if embedding fails
"""
struct EmbeddingError <: Exception
end

function _print_error(err,st)
    println()
    println(err)
    nlines=5
    for i=1:min(nlines,length(st))
        line=@sprintf("%s",st[i])
        L=length(line)
        if L<80
            println(line)
        else
            print(line[1:35])
            print(" ... ")
            println(line[L-35:L])
        end
    end
    if length(st)>nlines
        println("...")
    end
    println()
end

################################################################
function _solve!(
    solution::AbstractMatrix{Tv}, # old time step solution resp. initial value
    oldsol::AbstractMatrix{Tv}, # old time step solution resp. initial value
    this::AbstractSystem{Tv}, # Finite volume system
    control::NewtonControl,
    tstep::Tv,
    log::Bool
) where Tv

    _complete!(this, create_newtonvectors=true)
    nlhistory=zeros(0)

    solution.=oldsol
    residual=this.residual
    update=this.update
    _initialize!(solution,this)
    SuiteSparse.UMFPACK.umf_ctrl[3+1]=control.umfpack_pivot_tolerance

    oldnorm=1.0
    converged=false
    if control.verbose
        @printf("    Start Newton iteration\n")
    end
    nlu=0
    nround=0
    lufact=nothing
    damp=control.damp_initial
    tolx=0.0
    rnorm=LinearAlgebra.norm(values(solution),1)

    for ii=1:control.max_iterations
        try
            eval_and_assemble(this,solution,oldsol,residual,tstep,edge_cutoff=control.edge_cutoff)
        catch err
            if (control.handle_exceptions)
                _print_error(err,stacktrace(catch_backtrace()))
                throw(AssemblyError())
            else
                rethrow(err)
            end
        end

        mtx=this.matrix

        nliniter=0
        # Sparse LU factorization
        # Here, we seem miss the possibility to re-use the
        # previous symbolic information
        # We however reuse the factorization control.max_lureuse times.
        if nlu==0
            try
                lufact=LinearAlgebra.lu(mtx)
            catch err
                if (control.handle_exceptions)
                    _print_error(err,stacktrace(catch_backtrace()))
                    throw(FactorizationError())
                else
                    rethrow(err)
                end
            end
            # LU triangular solve gives Newton update
            try
                ldiv!(values(update),lufact,values(residual))
            catch err
                if (control.handle_exceptions)
                    _print_error(err,stacktrace(catch_backtrace()))
                    throw(FactorizationError())
                else
                    rethrow(err)
                end
            end
        else
            # When reusing lu factorization, we may try to iterate
            # Genericly, this is advisable.
            if control.tol_linear <1.0
                (sol,history)= bicgstabl!(values(update),
                                          mtx,
                                          values(residual),
                                          1,
                                          Pl=lufact,
                                          reltol=control.tol_linear,
                                          max_mv_products=100,
                                          log=true)
                nliniter=history.iters
            else
                try
                    ldiv!(values(update),lufact,values(residual))
                catch err
                    if (control.handle_exceptions)
                        _print_error(err,stacktrace(catch_backtrace()))
                        throw(FactorizationError())
                    else
                        rethrow(err)
                    end
                end
            end
        end

        nlu=min(nlu+1,control.max_lureuse)
        solval=values(solution)
        solval.-=damp*values(update)
        damp=min(damp*control.damp_growth,1.0)
        norm=LinearAlgebra.norm(values(update),Inf)
        if tolx==0.0
            tolx=norm*control.tol_relative
        end

        dnorm=1.0
        rnorm_new=LinearAlgebra.norm(values(solution),1)
        if rnorm>1.0e-50
            dnorm=abs((rnorm-rnorm_new)/rnorm)
        end

        if dnorm<control.tol_round
            nround=nround+1
        else
            nround=0
        end

        push!(nlhistory,norm)
        if control.verbose
            if   control.tol_linear<1.0
                itstring=@sprintf("it=% 3d(% 2d)",ii,nliniter)
            else
                itstring=@sprintf("it=% 3d",ii)
            end
            if control.max_round>0
                @printf("    %s du=%.3e cont=%.3e dnorm=%.3e %d\n",itstring,norm, norm/oldnorm,dnorm,nround)
            else
                @printf("    %s du=%.3e cont=%.3e\n",itstring,norm, norm/oldnorm)
            end
        end
        if ii>1 &&  norm/oldnorm > 1.0/control.tol_mono
            converged=false
            break
        end

        if norm<control.tol_absolute || norm <tolx
            converged=true
            break
        end
        oldnorm=norm
        rnorm=rnorm_new

        if nround>control.max_round
            converged=true
            break
        end
    end
    if !converged
        throw(ConvergenceError())
    end
    if control.verbose
        @printf("    Newton iteration successful\n")
    end
    return nlhistory
end

################################################################
"""
$(SIGNATURES)

Main assembly method.

Evaluate solution with result in right hand side F and
assemble matrix into system.matrix.
"""
function eval_and_assemble(system::AbstractSystem{Tv, Ti},
                           U::AbstractMatrix{Tv}, # Actual solution iteration
                           UOld::AbstractMatrix{Tv}, # Old timestep solution
                           F::AbstractMatrix{Tv},# Right hand side
                           tstep::Tv; # time step size. Inf means stationary solution
                           edge_cutoff=0.0
                           ) where {Tv, Ti}

    _complete!(system) # needed here as well for test function system which does not use newton

    grid=system.grid
    physics=system.physics
    data=physics.data
    node=Node{Tv,Ti}(system)
    bnode=BNode{Tv,Ti}(system)
    edge=Edge{Tv,Ti}(system)
    nspecies=num_species(system)
    matrix=system.matrix
    cellnodefactors=system.cellnodefactors
    celledgefactors=system.celledgefactors
    bfacenodefactors=system.bfacenodefactors

    # splatting would work but costs allocations
    if isdata(data)
        issource=(physics.source!=nofunc)
        isreaction=(physics.reaction!=nofunc)
        isbreaction=(physics.breaction!=nofunc)
        isbstorage=(physics.bstorage!=nofunc)

        fluxwrap=function(y, u)
            y.=0
            physics.flux(y,u,edge,data)
            nothing
        end

        reactionwrap=function (y, u)
            y.=0
            ## for ii in ..  uu[node.speclist[ii]]=u[ii]
            physics.reaction(y,u,node,data)
            ## for ii in .. y[ii]=y[node.speclist[ii]]
            nothing
        end

        storagewrap= function(y, u)
            y.=0
            physics.storage(y,u,node,data)
            nothing
        end

        sourcewrap=function(y)
            y.=0
            physics.source(y,node,data)
            nothing
        end

         breactionwrap=function(y, u)
            y.=0
            physics.breaction(y,u,bnode,data)
            nothing
        end

        bstoragewrap=function(y, u)
            y.=0
            physics.bstorage(y,u,bnode,data)
            nothing
        end

    else
        issource=(physics.source!=nofunc2)
        isreaction=(physics.reaction!=nofunc2)
        isbreaction=(physics.breaction!=nofunc2)
        isbstorage=(physics.bstorage!=nofunc2)

        fluxwrap=function(y, u)
            y.=0
            physics.flux(y,u,edge)
            nothing
        end

        reactionwrap=function(y, u)
            y.=0
            ## for ii in ..  uu[node.speclist[ii]]=u[ii]
            physics.reaction(y,u,node)
            ## for ii in .. y[ii]=y[node.speclist[ii]]
            nothing
        end

        storagewrap= function(y, u)
            y.=0
            physics.storage(y,u,node)
            nothing
        end

        sourcewrap=function(y)
            y.=0
            physics.source(y,node)
            nothing
        end

         breactionwrap=function(y, u)
            y.=0
            physics.breaction(y,u,bnode)
            nothing
        end

        bstoragewrap=function(y, u)
            y.=0
            physics.bstorage(y,u,bnode)
            nothing
        end

    end

    # Reset matrix + rhs
    nzv=nonzeros(matrix)
    nzv.=0.0
    F.=0.0

    # structs holding diff results for storage, reaction,  flux ...
    result_r=DiffResults.DiffResult(Vector{Tv}(undef,nspecies),Matrix{Tv}(undef,nspecies,nspecies))
    result_s=DiffResults.DiffResult(Vector{Tv}(undef,nspecies),Matrix{Tv}(undef,nspecies,nspecies))
    result_flx=DiffResults.DiffResult(Vector{Tv}(undef,nspecies),Matrix{Tv}(undef,nspecies,2*nspecies))

    # Array holding function results
    Y=Array{Tv,1}(undef,nspecies)

    # Arrays for gathering solution data
    UK=Array{Tv,1}(undef,nspecies)
    UKOld=Array{Tv,1}(undef,nspecies)
    UKL=Array{Tv,1}(undef,2*nspecies)

    # array holding source term
    src=zeros(Tv,nspecies)

    # arrays holding storage terms for old solution
    oldstor=zeros(Tv,nspecies)
    res_react=zeros(Tv,nspecies)
    jac_react=zeros(Tv,nspecies,nspecies)
    oldbstor=zeros(Tv,nspecies)

    cfg_r=ForwardDiff.JacobianConfig(reactionwrap, Y, UK)
    cfg_s=ForwardDiff.JacobianConfig(storagewrap, Y, UK)
    cfg_br=ForwardDiff.JacobianConfig(breactionwrap, Y, UK)
    cfg_bs=ForwardDiff.JacobianConfig(bstoragewrap, Y, UK)
    cfg_flx=ForwardDiff.JacobianConfig(fluxwrap, Y, UKL)

    # Inverse of timestep
    # According to Julia documentation, 1/Inf=0 which
    # comes handy to write compact code here.
    tstepinv=1.0/tstep

    boundary_factors=system.boundary_factors
    boundary_values=system.boundary_values

    geom=grid[CellGeometries][1]
    csys=grid[CoordinateSystem]
    coord=grid[Coordinates]
    cellnodes=grid[CellNodes]
    cellregions=grid[CellRegions]
    bgeom=grid[BFaceGeometries][1]
    bfacenodes=grid[BFaceNodes]
    bfaceregions=grid[BFaceRegions]
    nbfaces=num_bfaces(grid)
    ncells=num_cells(grid)

    if haskey(grid,CellEdges)
        cellx=grid[CellEdges]
        edgenodes=grid[EdgeNodes]
        has_celledges=true
    else
        cellx=grid[CellNodes]
        edgenodes=local_celledgenodes(geom)
        has_celledges=false
    end

    # Main cell loop
    for icell=1:ncells
        for inode=1:num_nodes(geom)
            _fill!(node,cellnodes,cellregions,inode,icell)
            for ispec=1:nspecies
                # xx gather:
                # ii=0
                # for i region_spec.colptr[ireg]:region_spec.colptr[ireg+1]-1
                #    ispec=Fdof.rowval[idof]
                #    ii=ii+1
                #    node.speclist[ii]=ispec
                #    UK[ii]=U[ispec,K]
                #   UK[1:nspecies]=U[:,node.index]
                #   UKOld[1:nspecies]=UOld[:,node.index]
                # Evaluate source term
                UK[ispec]=U[ispec,node.index]
                UKOld[ispec]=UOld[ispec,node.index]
            end

            if issource
                sourcewrap(src)
            end

            if isreaction
                # Evaluate & differentiate reaction term if present
                Y.=zero(Tv)
                ForwardDiff.jacobian!(result_r,reactionwrap,Y,UK,cfg_r)
                res_react=DiffResults.value(result_r)
                jac_react=DiffResults.jacobian(result_r)
            end

            # Evaluate & differentiate storage term
            Y.=zero(Tv)
            ForwardDiff.jacobian!(result_s,storagewrap,Y,UK,cfg_s)
            res_stor=DiffResults.value(result_s)
            jac_stor=DiffResults.jacobian(result_s)

            # Evaluate storage term for old timestep
            oldstor.=zero(Tv)
            storagewrap(oldstor,UKOld)

            K=node.index

            for idof=_firstnodedof(F,K):_lastnodedof(F,K)
                ispec=_spec(F,idof,K)
                _add(F,idof,cellnodefactors[inode,icell]*(res_react[ispec]-src[ispec] + (res_stor[ispec]-oldstor[ispec])*tstepinv))
                for jdof=_firstnodedof(F,K):_lastnodedof(F,K)
                    jspec=_spec(F,jdof,K)
                    _addnz(matrix,idof,jdof,jac_react[ispec,jspec]+ jac_stor[ispec,jspec]*tstepinv,cellnodefactors[inode,icell])
                end
            end
        end
        for iedge=1:num_edges(geom)
            if abs(celledgefactors[iedge,icell])<edge_cutoff
                continue
            end

            _fill!(edge,cellx,edgenodes,cellregions,iedge,icell, has_celledges)

            #Set up argument for fluxwrap
            for ispec=1:nspecies
                UKL[ispec]=U[ispec,edge.node[1]]
                UKL[nspecies+ispec]=U[ispec,edge.node[2]]
            end

            Y.=zero(Tv)
            ForwardDiff.jacobian!(result_flx,fluxwrap,Y,UKL,cfg_flx)
            res=DiffResults.value(result_flx)
            jac=DiffResults.jacobian(result_flx)

            K=edge.node[1]
            L=edge.node[2]
            fac=celledgefactors[iedge,icell]
            for idofK=_firstnodedof(F,K):_lastnodedof(F,K)
                ispec=_spec(F,idofK,K)
                idofL=dof(F,ispec,L)
                if idofL==0
                    continue
                end
                _add(F,idofK,fac*res[ispec])
                _add(F,idofL,-fac*res[ispec])

                for jdofK=_firstnodedof(F,K):_lastnodedof(F,K)
                    jspec=_spec(F,jdofK,K)
                    jdofL=dof(F,jspec,L)
                    if jdofL==0
                        continue
                    end

                    _addnz(matrix,idofK,jdofK,+jac[ispec,jspec         ],fac)
                    _addnz(matrix,idofL,jdofK,-jac[ispec,jspec         ],fac)
                    _addnz(matrix,idofK,jdofL,+jac[ispec,jspec+nspecies],fac)
                    _addnz(matrix,idofL,jdofL,-jac[ispec,jspec+nspecies],fac)

                end
            end
        end
    end

    # Assembly loop for boundary conditions
    for ibface=1:nbfaces
        # Obtain boundary region number
        ibreg=bfaceregions[ibface]

        # Loop over nodes of boundary face
        for ibnode=1:num_nodes(bgeom)

            # Fill bnode data shuttle with data from grid
            _fill!(bnode,bfacenodes,bfaceregions,ibnode,ibface)

            # Copy unknown values from solution into dense array
            for ispec=1:nspecies
                UK[ispec]=U[ispec,bnode.index]
            end

            # Measure of boundary face part assembled to node
            bnode_factor=bfacenodefactors[ibnode,ibface]

            # Global index of node
            K=bnode.index

            # Assemble "standard" boundary conditions: Robin or
            # Dirichlet
            # valid only for interior species, currently not checked
            for ispec=1:nspecies
                idof=dof(F,ispec,K)
                # If species is present, assemble the boundary condition
                if idof>0
                    # Get user specified data
                    boundary_factor=boundary_factors[ispec,ibreg]
                    boundary_value=boundary_values[ispec,ibreg]

                    if boundary_factor==Dirichlet
                        # Dirichlet is encoded in the boundary condition factor
                        # Penalty method: scale   (u-boundary_value) by penalty=boundary_factor

                        # Add penalty*boundry_value to right hand side
                        F[ispec,K]+=boundary_factor*(U[ispec,K]-boundary_value)

                        # Add penalty to matrix main diagonal (without bnode factor, so penalty
                        # is independent of h)
                        _addnz(matrix,idof,idof,boundary_factor,1)
                    else
                        # Robin boundary condition
                        F[ispec,K]+=bnode_factor*(boundary_factor*U[ispec,K]-boundary_value)
                        _addnz(matrix,idof,idof,boundary_factor, bnode_factor)
                    end
                end
            end

            # Boundary reaction term has been given.
            # Valid for both boundary and interior species
            if isbreaction

                # Evaluate function + derivative
                Y.=zero(Tv)
                ForwardDiff.jacobian!(result_r,breactionwrap,Y,UK,cfg_br)
                res_breact=DiffResults.value(result_r)
                jac_breact=DiffResults.jacobian(result_r)

                # Assemble RHS + matrix
                for idof=_firstnodedof(F,K):_lastnodedof(F,K)
                    ispec=_spec(F,idof,K)
                    _add(F,idof,bnode_factor*res_breact[ispec])
                    for jdof=_firstnodedof(F,K):_lastnodedof(F,K)
                        jspec=_spec(F,jdof,K)
                        _addnz(matrix,idof,jdof,jac_breact[ispec,jspec],bnode_factor)
                    end
                end

            end

            # Boundary reaction term has been given.
            # Valid only for boundary species, but this is currently not checked.
            if isbstorage

                # Fetch data for old timestep
                for ispec=1:nspecies
                    UKOld[ispec]=UOld[ispec,K]
                end
                # Evaluate storage term for old timestep
                bstoragewrap(oldbstor,UKOld)

                # Evaluate & differentiate storage term for new time step value
                Y.=zero(Tv)
                ForwardDiff.jacobian!(result_s,bstoragewrap,Y,UK,cfg_bs)
                res_bstor=DiffResults.value(result_s)
                jac_bstor=DiffResults.jacobian(result_s)

                for idof=_firstnodedof(F,K):_lastnodedof(F,K)
                    ispec=_spec(F,idof,K)
                    # Assemble finite difference in time for right hand side
                    _add(F,idof,bnode_factor*(res_bstor[ispec]-oldbstor[ispec])*tstepinv)
                    # Assemble matrix.
                    for jdof=_firstnodedof(F,K):_lastnodedof(F,K)
                        jspec=_spec(F,jdof,K)
                        _addnz(matrix,idof,jdof,jac_bstor[ispec,jspec],bnode_factor*tstepinv)
                    end
                end
            end

        end
    end
    _eval_and_assemble_generic_operator(system,U,F)
    _eval_and_assemble_inactive_species(system,U,UOld,F)
end

function _eval_and_assemble_generic_operator(this::AbstractSystem,U,F)

    if !has_generic_operator(this)
        return
    end
    generic_operator(f,u)=this.physics.generic_operator(f,u,this)
    vecF=values(F)
    vecU=values(U)
    y=similar(vecF)
    generic_operator(y,vecU)
    vecF.+=y
    forwarddiff_color_jacobian!(this.generic_matrix, generic_operator, vecU, colorvec = this.generic_matrix_colors)
    rowval=this.generic_matrix.rowval
    colptr=this.generic_matrix.colptr
    nzval=this.generic_matrix.nzval
    for i=1:length(colptr)-1
        for j=colptr[i]:colptr[i+1]-1
            updateindex!(this.matrix,+,nzval[j],i,rowval[j])
        end
    end
end

################################################################
"""
````
solve!(solution, inival, system; control=NewtonControl(),tstep=Inf, log=false)
````

Perform solution of stationary problem(if `tstep==Inf`) or one step
of the implicit Euler method using Newton's method with `inival` as initial
value. The method writes into `solution`.

It returns `solution` or, if `log==true`, a tuple of solution and a vector
containing the residual history of Newton's method.
"""
function solve!(
    solution::AbstractMatrix{Tv}, # Solution
    inival::AbstractMatrix{Tv},   # Initial value
    this::AbstractSystem{Tv};     # Finite volume system
    control=NewtonControl(),      # Newton solver control information
    tstep::Tv=Inf,                # Time step size. Inf means  stationary solution
    log::Bool=false
) where Tv
    if control.verbose
        @time begin
            history=_solve!(solution,inival,this,control,tstep,log)
        end
    else
        history=_solve!(solution,inival,this,control,tstep,log)
    end
    return log ? (solution,history) : solution
end

################################################################
"""
````
solve(inival, system; control=NewtonControl(), tstep=Inf, log=false)
````

Perform solution of stationary problem(if `tstep==Inf`) or one step
of the implicit Euler method using Newton's method with `inival` as initial
value. The method writes into `solution`.

It returns a solution array or, if `log==true`, a tuple of solution and a vector
containing the residual history of Newton's method.
"""
function solve(
    inival::AbstractMatrix{Tv},   # Initial value
    this::AbstractSystem{Tv};     # Finite volume system
    control=NewtonControl(),      # Newton solver control information
    tstep::Tv=Inf,                # Time step size. Inf means  stationary solution
    log::Bool=false
) where Tv
    solve!(unknowns(this),inival,this,control=control, tstep=tstep,log=log)
end

################################################################
"""
````
function embed!(solution, inival, system;
                control=NewtonControl(),
                pre=function(sol,p) end,
                post=function(sol,p) end
)
````
Solve stationary problem via parameter embedding, calling
`solve!` for increasing values of the parameter p from interval
(0,1). The user is responsible for the interpretation of
the parameter. The optional `pre()` callback can be used
to communicate its value to the system.
The optional `post()` callback method can be used to perform
various postprocessing steps.

If `control.handle_error` is true, `solve!`  throws an error, and
stepsize `control.Δp` is lowered,
and `solve!` is called again with a smaller  parameter
value. If `control.Δp<control.Δp_min`, `embed!` is aborted
with error.

"""
function embed!(
    solution::AbstractMatrix{Tv}, # Solution
    xinival::AbstractMatrix{Tv},   # Initial value
    this::AbstractSystem{Tv};     # Finite volume system
    control=NewtonControl(),      # Newton solver control information
    pre=function(sol,p) end,       # Function for preparing step
    post=function(sol,p) end      # Function for postprocessing successful step
) where Tv
    inival=copy(inival)
    p=0.0
    Δp=control.Δp
    if control.verbose
        @printf("  Embedding: start\n")
    end
    istep=0
    while p<1.0
        solved=false
        p0=p
        while !solved
            solved=true
            try
                p=min(p0+Δp,1.0)
                pre(solution,p)
                solve!(solution,inival, this ,control=control)
            catch err
                if (control.handle_exceptions)
                    _print_error(err,stacktrace(catch_backtrace()))
                else
                    rethrow(err)
                end
                # here we reduce the embedding step and retry the solution
                Δp=Δp*0.5
                if Δp<control.Δp_min
                    throw(EmbeddingError())
                end
                solved=false
                if control.verbose
                    @printf("  Embedding: retry: Δp=%.3e\n",Δp)
                end
            end
        end
        istep=istep+1
        if control.verbose
            @printf("  Embedding: istep=%d p=%.3e\n",istep,p)
        end
        Δp=min(control.Δp_max,Δp*1.2)
        post(solution,p)
        inival.=solution
    end
    if control.verbose
        @printf("  Embedding: success\n")
    end
    return solution
end

################################################################
"""
````
function evolve!(solution, inival, system, times;
                 control=NewtonControl(),
                 pre=function(sol,t) end,
                 post=function(sol,oldsol, t, Δt) end,
                 sample=function(sol,t) end,
                 delta=(u,v,t, Δt)->norm(sys,u-v,Inf)
)
````

Use implicit Euler method  + damped   Newton's method  to
solve time dependent problem. Time step control is performed
according to the data in `control`.  All times in `times`
are reached exactly.

Callbacks:
- `pre` is invoked before each time step
- `post`  is invoked after each time step
- `sample` is called for all times in `times[2:end]`.

`delta` is  used to control the time step.
"""
function evolve!(
    solution::AbstractMatrix{Tv}, # Solution
    inival::AbstractMatrix{Tv},   # Initial value
    this::AbstractSystem{Tv},    # Finite volume system
    times::AbstractVector;
    control=NewtonControl(),      # Newton solver control information
    pre=function(sol,t) end,       # Function for preparing step
    post=function(sol,oldsol, t, Δt) end,      # Function for postprocessing successful step
    sample=function(sol,t) end,      # Function to be called for each t\in times[2:end]
    delta=(u,v,t, Δt)->norm(sys,u-v,Inf) # Time step error estimator
) where Tv
    inival=copy(inival)
    _initialize_dirichlet!(inival,this)
    Δt=control.Δt
    if control.verbose
        @printf("  Evolution: start\n")
    end
    for i=1:length(times)-1

        Δt=max(Δt,control.Δt_min)
        tstart=times[i]
        tend=times[i+1]
        t=tstart
        istep=0

        while t<tend
            solved=false
            t0=t
            Δu=0.0
            while !solved
                solved=true
                try
                    t=t0+Δt
                    pre(solution,t)
                    solve!(solution,inival, this ,control=control,tstep=Δt)
                catch err
                    if (control.handle_exceptions)
                        _print_error(err,stacktrace(catch_backtrace()))
                    else
                        rethrow(err)
                    end
                    solved=false
                end
                if solved
                    Δu=delta(solution, inival,t, Δt)
                    if Δu>2.0*control.Δu_opt
                        solved=false
                    end
                end
                if !solved
                    # reduce time step and retry  solution
                    Δt=Δt*0.5
                    if Δt<control.Δt_min
                        @printf(" Δt_min=%.2g reached while Δu=%.2g >>  Δu_opt=%.2g\n",control.Δt_min, Δu,control.Δu_opt)
                        if !(control.force_first_step && istep==0)
                            throw(EmbeddingError())
                        else
                            solved=true
                        end
                    end
                    if control.verbose
                        @printf("  Evolution: retry: Δt=%.3e\n",Δt)
                    end
                end
            end
            istep=istep+1
            if control.verbose
                @printf("  Evolution: istep=%d t=%.3e Δu=%.3e\n",istep,t,Δu)
            end
            post(solution,inival,t, Δt)
            inival.=solution
            if t<tend
                Δt=min(control.Δt_max,
                       Δt*control.Δt_grow,
                       Δt*control.Δu_opt/(Δu+1.0e-14),
                       tend-t)
            end
        end
        sample(solution,times[i+1])
    end
    if control.verbose
        @printf("  Evolution: success\n")
    end
    return solution
end

"""
````
function solve(inival, system, times;
               control=NewtonControl(),
               pre=function(sol,t) end,
               post=function(sol,oldsol, t, Δt) end,
               sample=function(sol,t) end,
               delta=(u,v,t, Δt)->norm(sys,u-v,Inf),
               store_all=true,
               in_memory=true
)
````
Use implicit Euler method  + damped   Newton's method  to
solve time dependent problem. Time step control is performed
according to the data in `control`.  All times in `times`
are reached exactly.

Callbacks:
- `pre` is invoked before each time step
- `post`  is invoked after each time step
- `sample` is called for all times in `times[2:end]`.

`delta` is  used to control the time step.

If `store_all==true`, all timestep solutions are stored. Otherwise,
only solutions for the elements of `times` are stored.

Returns a transient solution object `sol` containing stored solutions,
see [`TransientSolution`](@ref)

"""
function solve(inival::AbstractMatrix,
               sys::AbstractSystem,
               times::AbstractVector;
               control=NewtonControl(),
               pre=function(sol,t) end,       # Function for preparing step
               post=function(sol,oldsol, t, Δt) end,      # Function for postprocessing successful step
               sample=function(sol,t) end,      # Function to be called for each t\in times[2:end]
               delta=(u,v,t, Δt)->norm(sys,u-v,Inf), # Time step error estimator
               store_all=true, # if true, store all solutions, otherwise, store only at sampling times
               in_memory=true
               )
    tsol=TransientSolution(Float64(times[1]),inival, in_memory=in_memory)
    solution=copy(inival)
    if store_all
        evolve!(solution,inival,sys,times,
                control=control,
                post=(u,uold,t,Δt)->(post(u,uold,t,Δt);append!(tsol,t,u)),
                pre=pre,
                sample=sample,
                delta=delta
                )
    else
        evolve!(solution,inival,sys,times,
                control=control,
                post=post,
                sample=(u,t)->(sample(u,t);append!(tsol,t,u)),
                pre=pre,
                delta=delta
                )
    end
    tsol
end

################################################################
"""
````
integrate(system,F,U; boundary=false)
````

Integrate node function (same signature as reaction or storage)
 `F` of  solution vector over domain or boundary
The result contains the integral for each species separately.
"""
function integrate(this::AbstractSystem{Tv,Ti,Tm},F::Function,U::AbstractMatrix{Tu}; boundary=false) where {Tu,Tv,Ti,Tm}
    grid=this.grid
    data=this.physics.data
    nspecies=num_species(this)
    res=zeros(Tu,nspecies)
    node=Node{Tv,Ti}(this)
    nodeparams=(node,)
    if isdata(data)
        nodeparams=(node,data,)
    end

    csys=grid[CoordinateSystem]
    coord=grid[Coordinates]

    if boundary

        geom=grid[BFaceGeometries][1]
        bfacenodes=grid[BFaceNodes]
        bfaceregions=grid[BFaceRegions]
        nbfaceregions=maximum(bfaceregions)
        integral=zeros(Tu,nspecies,nbfaceregions)

        for ibface=1:num_bfaces(grid)
            for inode=1:num_nodes(geom)
                _fill!(node,bfacenodes,bfaceregions,inode,ibface)
                res.=zero(Tv)
                @views F(res,U[:,node.index],nodeparams...)
                for ispec=1:nspecies
                    if this.node_dof[ispec,node.index]==ispec
                        integral[ispec,node.region]+=this.bfacenodefactors[inode,ibface]*res[ispec]
                    end
                end
            end
        end
    else
        geom=grid[CellGeometries][1]
        cellnodes=grid[CellNodes]
        cellregions=grid[CellRegions]
        ncellregions=maximum(cellregions)
        integral=zeros(Tu,nspecies,ncellregions)

        for icell=1:num_cells(grid)
            for inode=1:num_nodes(geom)
                _fill!(node,cellnodes,cellregions,inode,icell)
                res.=zero(Tv)
                @views F(res,U[:,node.index],nodeparams...)
                for ispec=1:nspecies
                    if this.node_dof[ispec,node.index]==ispec
                        integral[ispec,node.region]+=this.cellnodefactors[inode,icell]*res[ispec]
                    end
                end
            end
        end
    end

    return integral
end


text:
"""
    planar quadruped
    q = (x, z, t, q1, ..., q8)
        x - lateral position
        z - vertical position
        t - body orientation
        q1 - leg 1 shoulder angle (absolute)
        q2 - leg 1 knee angle     (absolute)
        q3 - leg 2 shoulder angle (absolute)
        q4 - leg 2 knee angle     (absolute)
        q5 - leg 3 shoulder angle (absolute)
        q6 - leg 3 knee angle     (absolute)
        q7 - leg 4 shoulder angle (absolute)
        q8 - leg 4 knee angle     (absolute)
"""
mutable struct QuadrupedSpring{T} <: Model{T}
    # dimensions
	nq::Int # generalized coordinates
    nu::Int # controls
    nw::Int # parameters
    nc::Int # contact points

    # parameters

    # torso
    l_torso1::T
    l_torso2::T
    m_torso::T
    J_torso::T

    # leg 1
        # thigh
    l_thigh1::T
    lc_thigh1::T
    m_thigh1::T
    J_thigh1::T
        # calf
    l_calf1::T
    lc_calf1::T
    m_calf1::T
    J_calf1::T

    # leg 2
        # thigh
    l_thigh2::T
    lc_thigh2::T
    m_thigh2::T
    J_thigh2::T
        # calf
    l_calf2::T
    lc_calf2::T
    m_calf2::T
    J_calf2::T

	# leg 3
        # thigh
    l_thigh3::T
    lc_thigh3::T
    m_thigh3::T
    J_thigh3::T
        # calf
    l_calf3::T
    lc_calf3::T
    m_calf3::T
    J_calf3::T

	# leg 4
        # thigh
    l_thigh4::T
    lc_thigh4::T
    m_thigh4::T
    J_thigh4::T
        # calf
    l_calf4::T
    lc_calf4::T
    m_calf4::T
    J_calf4::T

    # joint friction
	friction_joint::Vector{T}
	spring_stiffness_joint::Vector{T}
	spring_offset::Vector{T}

    # environment
    friction_body_world::Vector{T}
    friction_foot_world::Vector{T}
    gravity::T
end

function kinematics_hip(model::QuadrupedSpring, q; hip=:none)
	x = q[1]
	z = q[2]
    t_torso = q[3]

    if hip == :hip1
        l = model.l_torso1
    elseif hip == :hip2
        l = -model.l_torso2
    end

    return [
            x + l * cos(t_torso);
            z + l * sin(t_torso)
           ]
end

function kinematics_jacobian_hip(model::QuadrupedSpring, q; hip=:none)
	x = q[1]
	z = q[2]
    t_torso = q[3]

    if hip == :hip1
        l = model.l_torso1
    elseif hip == :hip2
        l = -model.l_torso2
    end

    jac = zeros(eltype(q), 2, model.nq)

    jac[1, 1] = 1.0
    jac[1, 3] = -l * sin(t_torso)
    jac[2, 2] = 1.0
    jac[2, 3] = l * cos(t_torso)

    return jac
end

function kinematics_thigh(model::QuadrupedSpring, q; leg=:none, mode = :ee)
	x = q[1]
	z = q[2]
    t_torso = q[3]

    if leg == :leg1
        t_shoulder = q[4]
        l_torso = model.l_torso1
        le_thigh = model.l_thigh1
        lc_thigh = model.lc_thigh1
        hip = :hip1
    elseif leg == :leg2
        t_shoulder = q[6]
        l_torso = model.l_torso1
        le_thigh = model.l_thigh2
        lc_thigh = model.lc_thigh2
        hip = :hip1
    elseif leg == :leg3
        t_shoulder = q[8]
        l_torso = -model.l_torso2
        le_thigh = model.l_thigh3
        lc_thigh = model.lc_thigh3
        hip = :hip2
    elseif leg == :leg4
        t_shoulder = q[10]
        l_torso = -model.l_torso2
        le_thigh = model.l_thigh4
        lc_thigh = model.lc_thigh4
        hip = :hip2
    else
        @error "incorrect leg specified"
    end

    if mode == :ee
        l_thigh = le_thigh
    elseif mode == :com
        l_thigh = lc_thigh
    else
        @error "incorrect mode specified"
    end
    k_hip = kinematics_hip(model, q, hip=hip)

    return k_hip + [l_thigh * sin(t_shoulder);
                   -l_thigh * cos(t_shoulder)]
end

function kinematics_jacobian_thigh(model::QuadrupedSpring, q; leg=:none, mode = :ee)
	x = q[1]
	z = q[2]
    t_torso = q[3]

    if leg == :leg1
        idx = 4
        l_torso = model.l_torso1
        le_thigh = model.l_thigh1
        lc_thigh = model.lc_thigh1
        hip = :hip1
    elseif leg == :leg2
        idx = 6
        l_torso = model.l_torso1
        le_thigh = model.l_thigh2
        lc_thigh = model.lc_thigh2
        hip = :hip1
    elseif leg == :leg3
        idx = 8
        l_torso = -model.l_torso2
        le_thigh = model.l_thigh3
        lc_thigh = model.lc_thigh3
        hip = :hip2
    elseif leg == :leg4
        idx = 10
        l_torso = -model.l_torso2
        le_thigh = model.l_thigh4
        lc_thigh = model.lc_thigh4
        hip = :hip2
    else
        @error "incorrect leg specified"
    end

    t_shoulder = q[idx]

    if mode == :ee
        l_thigh = le_thigh
    elseif mode == :com
        l_thigh = lc_thigh
    else
        @error "incorrect mode specified"
    end

    jac = kinematics_jacobian_hip(model, q, hip=hip)

    jac[1, idx] += l_thigh * cos(t_shoulder)
    jac[2, idx] += l_thigh * sin(t_shoulder)

    return jac
end

function kinematics_calf(model::QuadrupedSpring, q; leg=:none, mode = :none)
	x = q[1]
	z = q[2]
    t_torso = q[3]

    if leg == :leg1
        idx = 4
        l_torso = model.l_torso1
        l_thigh = model.l_thigh1
        le_calf = model.l_calf1
        lc_calf = model.lc_calf1
    elseif leg == :leg2
        idx = 6
        l_torso = model.l_torso1
        l_thigh = model.l_thigh2
        le_calf = model.l_calf2
        lc_calf = model.lc_calf2
    elseif leg == :leg3
        idx = 8
        l_torso = -model.l_torso2
        l_thigh = model.l_thigh3
        le_calf = model.l_calf3
        lc_calf = model.lc_calf3
    elseif leg == :leg4
        idx = 10
        l_torso = -model.l_torso2
        l_thigh = model.l_thigh4
        le_calf = model.l_calf4
        lc_calf = model.lc_calf4
    else
        @error "incorrect leg specified"
    end

    t_shoulder = q[idx]
    t_calf = q[idx + 1]

    if mode == :ee
        l_calf = le_calf
    elseif mode == :com
        l_calf = lc_calf
    else
        @error "incorrect mode specified"
    end

    k_thigh = kinematics_thigh(model, q, leg=leg, mode=:ee)

    return k_thigh + [l_calf * sin(t_calf);
                     -l_calf * cos(t_calf)]
end

function kinematics_jacobian_calf(model::QuadrupedSpring, q; leg=:none, mode = :none)
	x = q[1]
	z = q[2]
    t_torso = q[3]

    if leg == :leg1
        idx = 4
        l_torso = model.l_torso1
        l_thigh = model.l_thigh1
        le_calf = model.l_calf1
        lc_calf = model.lc_calf1
    elseif leg == :leg2
        idx = 6
        l_torso = model.l_torso1
        l_thigh = model.l_thigh2
        le_calf = model.l_calf2
        lc_calf = model.lc_calf2
    elseif leg == :leg3
        idx = 8
        l_torso = -model.l_torso2
        l_thigh = model.l_thigh3
        le_calf = model.l_calf3
        lc_calf = model.lc_calf3
    elseif leg == :leg4
        idx = 10
        l_torso = -model.l_torso2
        l_thigh = model.l_thigh4
        le_calf = model.l_calf4
        lc_calf = model.lc_calf4
    else
        @error "incorrect leg specified"
    end

    t_shoulder = q[idx]
    t_calf = q[idx + 1]

    if mode == :ee
        l_calf = le_calf
    elseif mode == :com
        l_calf = lc_calf
    else
        @error "incorrect mode specified"
    end

    jac = kinematics_jacobian_thigh(model, q, leg=leg, mode=:ee)
    jac[1, idx + 1] += l_calf * cos(t_calf)
    jac[2, idx + 1] += l_calf * sin(t_calf)

    return jac
end

function kinematics(model::QuadrupedSpring, q)
	p_foot_1 = kinematics_calf(model, q, leg=:leg1, mode=:ee) # foot 1
	p_foot_2 = kinematics_calf(model, q, leg=:leg2, mode=:ee) # foot 2
	p_foot_3 = kinematics_calf(model, q, leg=:leg3, mode=:ee) # foot 3
	p_foot_4 = kinematics_calf(model, q, leg=:leg4, mode=:ee) # foot 4

    # p_knee_1 = kinematics_thigh(model, q, leg=:leg1, mode=:ee) # knee 1
	# p_knee_2 = kinematics_thigh(model, q, leg=:leg2, mode=:ee) # knee 2
	# p_knee_3 = kinematics_thigh(model, q, leg=:leg3, mode=:ee) # knee 3
	# p_knee_4 = kinematics_thigh(model, q, leg=:leg4, mode=:ee) # knee 4

    # p_hip_1 = kinematics_hip(model, q, hip=:hip1) # hip 1
	# p_hip_2 = kinematics_hip(model, q, hip=:hip2) # hip 2

	[
     p_foot_1; p_foot_2; p_foot_3; p_foot_4;
    #  p_knee_1; p_knee_2; p_knee_3; p_knee_4;
    #  p_hip_1; p_hip_2
    ]
end

# Lagrangian
function lagrangian(model::QuadrupedSpring, q, q̇)
	L = 0.0

	# torso
	p_torso = q[1:2]
	v_torso = q̇[1:2]

	L += 0.5 * model.m_torso * transpose(v_torso) * v_torso
	L += 0.5 * model.J_torso * q̇[3]^2.0
	L -= model.m_torso * model.gravity * p_torso[2]

	# thigh 1
	p_thigh_1 = kinematics_thigh(model, q, leg=:leg1, mode=:com)
	J_thigh_1 = kinematics_jacobian_thigh(model, q, leg=:leg1, mode=:com)
	v_thigh_1 = J_thigh_1 * q̇

	L += 0.5 * model.m_thigh1 * transpose(v_thigh_1) * v_thigh_1
	L += 0.5 * model.J_thigh1 * q̇[4]^2.0
	L -= model.m_thigh1 * model.gravity * p_thigh_1[2]

	# calf 1
	p_calf_1 = kinematics_calf(model, q, leg=:leg1, mode=:com)
	J_calf_1 = kinematics_jacobian_calf(model, q, leg=:leg1, mode=:com)
	v_calf_1 = J_calf_1 * q̇

	L += 0.5 * model.m_calf1 * transpose(v_calf_1) * v_calf_1
	L += 0.5 * model.J_calf1 * q̇[5]^2.0
	L -= model.m_calf1 * model.gravity * p_calf_1[2]

	# thigh 2
	p_thigh_2 = kinematics_thigh(model, q, leg=:leg2, mode=:com)
	J_thigh_2 = kinematics_jacobian_thigh(model, q, leg=:leg2, mode=:com)
	v_thigh_2 = J_thigh_2 * q̇

	L += 0.5 * model.m_thigh2 * transpose(v_thigh_2) * v_thigh_2
	L += 0.5 * model.J_thigh2 * q̇[6]^2.0
	L -= model.m_thigh2 * model.gravity * p_thigh_2[2]

	# calf 2
	p_calf_2 = kinematics_calf(model, q, leg=:leg2, mode=:com)
	J_calf_2 = kinematics_jacobian_calf(model, q, leg=:leg2, mode=:com)
	v_calf_2 = J_calf_2 * q̇

	L += 0.5 * model.m_calf2 * transpose(v_calf_2) * v_calf_2
	L += 0.5 * model.J_calf2 * q̇[7]^2.0
	L -= model.m_calf2 * model.gravity * p_calf_2[2]

	# thigh 3
	p_thigh_3 = kinematics_thigh(model, q, leg=:leg3, mode=:com)
	J_thigh_3 = kinematics_jacobian_thigh(model, q, leg=:leg3, mode=:com)
	v_thigh_3 = J_thigh_3 * q̇

	L += 0.5 * model.m_thigh3 * transpose(v_thigh_3) * v_thigh_3
	L += 0.5 * model.J_thigh3 * q̇[8]^2.0
	L -= model.m_thigh3 * model.gravity * p_thigh_3[2]

	# calf 3
	p_calf_3 = kinematics_calf(model, q, leg=:leg3, mode=:com)
	J_calf_3 = kinematics_jacobian_calf(model, q, leg=:leg3, mode=:com)
	v_calf_3 = J_calf_3 * q̇

	L += 0.5 * model.m_calf3 * transpose(v_calf_3) * v_calf_3
	L += 0.5 * model.J_calf3 * q̇[9]^2.0
	L -= model.m_calf3 * model.gravity * p_calf_3[2]

	# thigh 4
	p_thigh_4 = kinematics_thigh(model, q, leg=:leg4, mode=:com)
	J_thigh_4 = kinematics_jacobian_thigh(model, q, leg=:leg4, mode=:com)
	v_thigh_4 = J_thigh_4 * q̇

	L += 0.5 * model.m_thigh4 * transpose(v_thigh_4) * v_thigh_4
	L += 0.5 * model.J_thigh4 * q̇[10]^2.0
	L -= model.m_thigh4 * model.gravity * p_thigh_4[2]

	# calf 4
	p_calf_4 = kinematics_calf(model, q, leg=:leg4, mode=:com)
	J_calf_4 = kinematics_jacobian_calf(model, q, leg=:leg4, mode=:com)
	v_calf_4 = J_calf_4 * q̇

	L += 0.5 * model.m_calf4 * transpose(v_calf_4) * v_calf_4
	L += 0.5 * model.J_calf4 * q̇[11]^2.0
	L -= model.m_calf4 * model.gravity * p_calf_4[2]

	# add spring
	offsets = model.spring_offset
	spring = model.spring_stiffness_joint
	L -= 0.5 * spring[1] * (offsets[1] + q[3] - q[4])^2
	L -= 0.5 * spring[2] * (offsets[2] + q[4] - q[5])^2
	L -= 0.5 * spring[3] * (offsets[1] + q[3] - q[6])^2
	L -= 0.5 * spring[4] * (offsets[2] + q[6] - q[7])^2
	L -= 0.5 * spring[5] * (offsets[1] + q[3] - q[8])^2
	L -= 0.5 * spring[6] * (offsets[2] + q[8] - q[9])^2
	L -= 0.5 * spring[7] * (offsets[1] + q[3] - q[10])^2
	L -= 0.5 * spring[8] * (offsets[2] + q[10] - q[11])^2
	return L
end

function signed_distance(model::QuadrupedSpring, q)
	k = kinematics(model, q)
    return [
            k[2], k[4], k[6], k[8]
           ]
        #    , k[10], k[12], k[14], k[16], k[18], k[20]]
end

function input_jacobian(::QuadrupedSpring, q)
	[1.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0;
	 0.0  1.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0;
	 0.0  0.0  1.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0;
     0.0  0.0 -1.0  1.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0;
     0.0  0.0  0.0 -1.0  1.0  0.0  0.0  0.0  0.0  0.0  0.0;
     0.0  0.0 -1.0  0.0  0.0  1.0  0.0  0.0  0.0  0.0  0.0;
     0.0  0.0  0.0  0.0  0.0 -1.0  1.0  0.0  0.0  0.0  0.0;
     0.0  0.0 -1.0  0.0  0.0  0.0  0.0  1.0  0.0  0.0  0.0;
     0.0  0.0  0.0  0.0  0.0  0.0  0.0 -1.0  1.0  0.0  0.0;
     0.0  0.0 -1.0  0.0  0.0  0.0  0.0  0.0  0.0  1.0  0.0;
     0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0 -1.0  1.0]
end

function contact_jacobian(model::QuadrupedSpring, q)
    J1 = kinematics_jacobian_calf(model, q, leg=:leg1, mode=:ee)
	J2 = kinematics_jacobian_calf(model, q, leg=:leg2, mode=:ee)
	J3 = kinematics_jacobian_calf(model, q, leg=:leg3, mode=:ee)
	J4 = kinematics_jacobian_calf(model, q, leg=:leg4, mode=:ee)

    # J5 = kinematics_jacobian_thigh(model, q, leg=:leg1, mode=:ee)
	# J6 = kinematics_jacobian_thigh(model, q, leg=:leg2, mode=:ee)
	# J7 = kinematics_jacobian_thigh(model, q, leg=:leg3, mode=:ee)
	# J8 = kinematics_jacobian_thigh(model, q, leg=:leg4, mode=:ee)

    # J9  = kinematics_jacobian_hip(model, q, hip=:hip1)
    # J10 = kinematics_jacobian_hip(model, q, hip=:hip2)

    [
     J1;
     J2;
     J3;
     J4;
    #  J5;
    #  J6;
    #  J7;
    #  J8;
    #  J9;
    #  J10
    ]
end

# nominal configuration
function nominal_configuration(model::QuadrupedSpring)
    [0.0; 0.5; 0.0 * π; 0.25 * π; 0.5 * π; 0.1 * π; 0.3 * π; -0.25 * π; 0.1 * π; -0.5 * π; -0.1 * π]
end

# friction coefficients
function friction_coefficients(model::QuadrupedSpring)
    [model.friction_foot_world; model.friction_body_world]
end

# dimensions
nq = 3 + 2 * 4            # generalized coordinates
nu = 3 + 2 * 4            # controls
nw = 0                    # parameters
nc = 4                    # contact points

# parameters
gravity = 9.81            # gravity
friction_body_world = [0.5; 0.5; 0.5; 0.5; 0.5; 0.5] # coefficient of friction
friction_foot_world = [0.5; 0.5; 0.5; 0.5] # coefficient of friction
friction_joint = 0.2*[0.1; 0.1; 0.1; 0.1; 0.1; 0.1; 0.1; 0.1]
spring_stiffness_joint = 20*ones(8)
spring_offset = [-0.10-0.8823645679625642, 0.8823645679625642 + 0.6224325985648779]

# similar to Unitree A1
m_torso = 4.713 + 4 * 0.696
m_thigh = 1.013
m_calf = 0.166

J_torso = 0.01683 + 4 * 0.696 * 0.183^2.0
J_thigh = 0.00552
J_calf = 0.00299

l_torso1 = 0.183
l_torso2 = 0.183
l_thigh = 0.2
l_calf = 0.2

lc_thigh = 0.5 * l_thigh - 0.00323
lc_calf = 0.5 * l_calf - 0.006435

quadruped_spring = QuadrupedSpring(nq, nu, nw, nc,
				l_torso1, l_torso2, m_torso, J_torso,
				l_thigh, lc_thigh, m_thigh, J_thigh,
				l_calf, lc_calf, m_calf, J_calf,
				l_thigh, lc_thigh, m_thigh, J_thigh,
				l_calf, lc_calf, m_calf, J_calf,
				l_thigh, lc_thigh, m_thigh, J_thigh,
				l_calf, lc_calf, m_calf, J_calf,
				l_thigh, lc_thigh, m_thigh, J_thigh,
				l_calf, lc_calf, m_calf, J_calf,
	            friction_joint,
				spring_stiffness_joint,
				spring_offset,
                friction_body_world,
                friction_foot_world,
                gravity)

quadruped_spring_contact_kinematics = [
    q -> kinematics_calf(quadruped_spring, q, leg=:leg1, mode=:ee),
    q -> kinematics_calf(quadruped_spring, q, leg=:leg2, mode=:ee),
    q -> kinematics_calf(quadruped_spring, q, leg=:leg3, mode=:ee),
    q -> kinematics_calf(quadruped_spring, q, leg=:leg4, mode=:ee),
    # q -> kinematics_thigh(quadruped, q, leg=:leg1, mode=:ee),
    # q -> kinematics_thigh(quadruped, q, leg=:leg2, mode=:ee),
    # q -> kinematics_thigh(quadruped, q, leg=:leg3, mode=:ee),
    # q -> kinematics_thigh(quadruped, q, leg=:leg4, mode=:ee),
    # q -> kinematics_hip(quadruped, q, hip=:hip1),
    # q -> kinematics_hip(quadruped, q, hip=:hip2)
]

quadruped_spring_contact_kinematics_jacobians = [
    q -> kinematics_jacobian_calf(quadruped_spring, q, leg=:leg1, mode=:ee),
    q -> kinematics_jacobian_calf(quadruped_spring, q, leg=:leg2, mode=:ee),
    q -> kinematics_jacobian_calf(quadruped_spring, q, leg=:leg3, mode=:ee),
    q -> kinematics_jacobian_calf(quadruped_spring, q, leg=:leg4, mode=:ee),
    # q -> kinematics_jacobian_thigh(quadruped, q, leg=:leg1, mode=:ee),
    # q -> kinematics_jacobian_thigh(quadruped, q, leg=:leg2, mode=:ee),
    # q -> kinematics_jacobian_thigh(quadruped, q, leg=:leg3, mode=:ee),
    # q -> kinematics_jacobian_thigh(quadruped, q, leg=:leg4, mode=:ee),
    # q -> kinematics_jacobian_hip(quadruped, q, hip=:hip1),
    # q -> kinematics_jacobian_hip(quadruped, q, hip=:hip2)
]

name(::QuadrupedSpring) = :quadruped_spring
floating_base_dim(::QuadrupedSpring) = 3

ext: jl


text:
struct FLSynapseParameter
end

@snn_kw mutable struct FLSynapse{MFT=Matrix{Float32},VFT=Vector{Float32},FT=Float32}
    param::FLSynapseParameter = FLSynapseParameter()
    W::MFT  # synaptic weight
    rI::VFT # postsynaptic rate
    rJ::VFT # presynaptic rate
    g::VFT  # postsynaptic conductance
    P::MFT  # <rᵢrⱼ>⁻¹
    q::VFT  # P * r
    u::VFT # force weight
    w::VFT # output weight
    f::FT = 0 # postsynaptic traget
    z::FT = 0.5randn()  # output z ≈ f
    records::Dict = Dict()
end

"""
[Force Learning Full Synapse](http://www.theswartzfoundation.org/docs/Sussillo-Abbott-Coherent-Patterns-August-2009.pdf)
"""
FLSynapse

function FLSynapse(pre, post; σ = 1.5, p = 0.0, α = 1, kwargs...)
    rI, rJ, g = post.r, pre.r, post.g
    W = σ * 1 / √pre.N * randn(post.N, pre.N) # normalized recurrent weight
    w = 1 / √post.N * (2rand(post.N) .- 1) # initial output weight
    u = 2rand(post.N) .- 1 # initial force weight
    P = α * I(post.N) # initial inverse of C = <rr'>
    q = zeros(post.N)
    FLSynapse(;@symdict(W, rI, rJ, g, P, q, u, w)..., kwargs...)
end

function forward!(c::FLSynapse, param::FLSynapseParameter)
    @unpack W, rI, rJ, g, P, q, u, w, z = c
    z = dot(w, rI)
    mul!(q, P, rJ)
    mul!(g, W, rJ)
    axpy!(z, u, g)
end

function plasticity!(c::FLSynapse, param::FLSynapseParameter, dt::Float32, t::Float32)
    @unpack rI, P, q, w, f, z = c
    C = 1 / (1 + dot(q, rI))
    axpy!(C * (f - z), q, w)
    BLAS.ger!(-C, q, q, P)
end


text:
export BrickletLEDStripRGBValues
struct BrickletLEDStripRGBValues
    r::Vector{Integer}
    g::Vector{Integer}
    b::Vector{Integer}
end

export BrickletLEDStripRGBWValues
struct BrickletLEDStripRGBWValues
    r::Vector{Integer}
    g::Vector{Integer}
    b::Vector{Integer}
    w::Vector{Integer}
end

export BrickletLEDStripIdentity
struct BrickletLEDStripIdentity
    uid::String
    connected_uid::String
    position::Char
    hardware_version::Vector{Integer}
    firmware_version::Vector{Integer}
    device_identifier::Integer
end

export BrickletLEDStrip
"""
Controls up to 320 RGB LEDs
"""
mutable struct BrickletLEDStrip <: TinkerforgeDevice
    ipcon::IPConnection
    deviceInternal::PyObject

    """
    Creates an object with the unique device ID *uid* and adds it to
    the IP Connection *ipcon*.
    """
    function BrickletLEDStrip(uid::String, ipcon::IPConnection)
        package = pyimport("tinkerforge.bricklet_led_strip")
        deviceInternal = package.BrickletLEDStrip(uid, ipcon.ipconInternal)

        return new(ipcon, deviceInternal)
    end
end

export set_rgb_values
"""
    $(SIGNATURES)

Sets *length* RGB values for the LEDs starting from *index*.

To make the colors show correctly you need to configure the chip type
(:func:`Set Chip Type`) and a 3-channel channel mapping (:func:`Set Channel Mapping`)
according to the connected LEDs.

Example: If you set

* index to 5,
* length to 3,
* r to [255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
* g to [0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] and
* b to [0, 0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

the LED with index 5 will be red, 6 will be green and 7 will be blue.

.. note:: Depending on the LED circuitry colors can be permuted.

The colors will be transfered to actual LEDs when the next
frame duration ends, see :func:`Set Frame Duration`.

Generic approach:

* Set the frame duration to a value that represents
  the number of frames per second you want to achieve.
* Set all of the LED colors for one frame.
* Wait for the :cb:`Frame Rendered` callback.
* Set all of the LED colors for next frame.
* Wait for the :cb:`Frame Rendered` callback.
* and so on.

This approach ensures that you can change the LED colors with
a fixed frame rate.

The actual number of controllable LEDs depends on the number of free
Bricklet ports. See :ref:`here <led_strip_bricklet_ram_constraints>` for more
information. A call of :func:`Set RGB Values` with index + length above the
bounds is ignored completely.
"""
function set_rgb_values(device::BrickletLEDStrip, index, length, r, g, b)
    device.deviceInternal.set_rgb_values(index, length, r, g, b)
end

export get_rgb_values
"""
    $(SIGNATURES)

Returns *length* R, G and B values starting from the
given LED *index*.

The values are the last values that were set by :func:`Set RGB Values`.
"""
function get_rgb_values(device::BrickletLEDStrip, index, length)
    return device.deviceInternal.get_rgb_values(index, length)
end

export set_frame_duration
"""
    $(SIGNATURES)

Sets the frame duration.

Example: If you want to achieve 20 frames per second, you should
set the frame duration to 50ms (50ms * 20 = 1 second).

For an explanation of the general approach see :func:`Set RGB Values`.
"""
function set_frame_duration(device::BrickletLEDStrip, duration)
    device.deviceInternal.set_frame_duration(duration)
end

export get_frame_duration
"""
    $(SIGNATURES)

Returns the frame duration as set by :func:`Set Frame Duration`.
"""
function get_frame_duration(device::BrickletLEDStrip)
    return device.deviceInternal.get_frame_duration()
end

export get_supply_voltage
"""
    $(SIGNATURES)

Returns the current supply voltage of the LEDs.
"""
function get_supply_voltage(device::BrickletLEDStrip)
    return device.deviceInternal.get_supply_voltage()
end

export set_clock_frequency
"""
    $(SIGNATURES)

Sets the frequency of the clock.

The Bricklet will choose the nearest achievable frequency, which may
be off by a few Hz. You can get the exact frequency that is used by
calling :func:`Get Clock Frequency`.

If you have problems with flickering LEDs, they may be bits flipping. You
can fix this by either making the connection between the LEDs and the
Bricklet shorter or by reducing the frequency.

With a decreasing frequency your maximum frames per second will decrease
too.

.. note::
 The frequency in firmware version 2.0.0 is fixed at 2MHz.

.. versionadded:: 2.0.1\$nbsp;(Plugin)
"""
function set_clock_frequency(device::BrickletLEDStrip, frequency)
    device.deviceInternal.set_clock_frequency(frequency)
end

export get_clock_frequency
"""
    $(SIGNATURES)

Returns the currently used clock frequency as set by :func:`Set Clock Frequency`.

.. versionadded:: 2.0.1\$nbsp;(Plugin)
"""
function get_clock_frequency(device::BrickletLEDStrip)
    return device.deviceInternal.get_clock_frequency()
end

export set_chip_type
"""
    $(SIGNATURES)

Sets the type of the LED driver chip. We currently support the chips

* WS2801,
* WS2811,
* WS2812 / SK6812 / NeoPixel RGB,
* SK6812RGBW / NeoPixel RGBW (Chip Type = WS2812),
* LPD8806 and
* APA102 / DotStar.

.. versionadded:: 2.0.2\$nbsp;(Plugin)
"""
function set_chip_type(device::BrickletLEDStrip, chip)
    device.deviceInternal.set_chip_type(chip)
end

export get_chip_type
"""
    $(SIGNATURES)

Returns the currently used chip type as set by :func:`Set Chip Type`.

.. versionadded:: 2.0.2\$nbsp;(Plugin)
"""
function get_chip_type(device::BrickletLEDStrip)
    return device.deviceInternal.get_chip_type()
end

export set_rgbw_values
"""
    $(SIGNATURES)

Sets *length* RGBW values for the LEDs starting from *index*.

To make the colors show correctly you need to configure the chip type
(:func:`Set Chip Type`) and a 4-channel channel mapping (:func:`Set Channel Mapping`)
according to the connected LEDs.

The maximum length is 12, the index goes from 0 to 239 and the rgbw values
have 8 bits each.

Example: If you set

* index to 5,
* length to 4,
* r to [255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
* g to [0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
* b to [0, 0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0] and
* w to [0, 0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0]

the LED with index 5 will be red, 6 will be green, 7 will be blue and 8 will be white.

.. note:: Depending on the LED circuitry colors can be permuted.

The colors will be transfered to actual LEDs when the next
frame duration ends, see :func:`Set Frame Duration`.

Generic approach:

* Set the frame duration to a value that represents
  the number of frames per second you want to achieve.
* Set all of the LED colors for one frame.
* Wait for the :cb:`Frame Rendered` callback.
* Set all of the LED colors for next frame.
* Wait for the :cb:`Frame Rendered` callback.
* and so on.

This approach ensures that you can change the LED colors with
a fixed frame rate.

The actual number of controllable LEDs depends on the number of free
Bricklet ports. See :ref:`here <led_strip_bricklet_ram_constraints>` for more
information. A call of :func:`Set RGBW Values` with index + length above the
bounds is ignored completely.

The LPD8806 LED driver chips have 7-bit channels for RGB. Internally the LED
Strip Bricklets divides the 8-bit values set using this function by 2 to make
them 7-bit. Therefore, you can just use the normal value range (0-255) for
LPD8806 LEDs.

The brightness channel of the APA102 LED driver chips has 5-bit. Internally the
LED Strip Bricklets divides the 8-bit values set using this function by 8 to make
them 5-bit. Therefore, you can just use the normal value range (0-255) for
the brightness channel of APA102 LEDs.

.. versionadded:: 2.0.6\$nbsp;(Plugin)
"""
function set_rgbw_values(device::BrickletLEDStrip, index, length, r, g, b, w)
    device.deviceInternal.set_rgbw_values(index, length, r, g, b, w)
end

export get_rgbw_values
"""
    $(SIGNATURES)

Returns *length* RGBW values starting from the given *index*.

The values are the last values that were set by :func:`Set RGBW Values`.

.. versionadded:: 2.0.6\$nbsp;(Plugin)
"""
function get_rgbw_values(device::BrickletLEDStrip, index, length)
    return device.deviceInternal.get_rgbw_values(index, length)
end

export set_channel_mapping
"""
    $(SIGNATURES)

Sets the channel mapping for the connected LEDs.

:func:`Set RGB Values` and :func:`Set RGBW Values` take the data in RGB(W) order.
But the connected LED driver chips might have their 3 or 4 channels in a
different order. For example, the WS2801 chips typically use BGR order, the
WS2812 chips typically use GRB order and the APA102 chips typically use WBGR
order.

The APA102 chips are special. They have three 8-bit channels for RGB
and an additional 5-bit channel for the overall brightness of the RGB LED
making them 4-channel chips. Internally the brightness channel is the first
channel, therefore one of the Wxyz channel mappings should be used. Then
the W channel controls the brightness.

If a 3-channel mapping is selected then :func:`Set RGB Values` has to be used.
Calling :func:`Set RGBW Values` with a 3-channel mapping will produce incorrect
results. Vice-versa if a 4-channel mapping is selected then
:func:`Set RGBW Values` has to be used. Calling :func:`Set RGB Values` with a
4-channel mapping will produce incorrect results.

.. versionadded:: 2.0.6\$nbsp;(Plugin)
"""
function set_channel_mapping(device::BrickletLEDStrip, mapping)
    device.deviceInternal.set_channel_mapping(mapping)
end

export get_channel_mapping
"""
    $(SIGNATURES)

Returns the currently used channel mapping as set by :func:`Set Channel Mapping`.

.. versionadded:: 2.0.6\$nbsp;(Plugin)
"""
function get_channel_mapping(device::BrickletLEDStrip)
    return device.deviceInternal.get_channel_mapping()
end

export enable_frame_rendered_callback
"""
    $(SIGNATURES)

Enables the :cb:`Frame Rendered` callback.

By default the callback is enabled.

.. versionadded:: 2.0.6\$nbsp;(Plugin)
"""
function enable_frame_rendered_callback(device::BrickletLEDStrip)
    device.deviceInternal.enable_frame_rendered_callback()
end

export disable_frame_rendered_callback
"""
    $(SIGNATURES)

Disables the :cb:`Frame Rendered` callback.

By default the callback is enabled.

.. versionadded:: 2.0.6\$nbsp;(Plugin)
"""
function disable_frame_rendered_callback(device::BrickletLEDStrip)
    device.deviceInternal.disable_frame_rendered_callback()
end

export is_frame_rendered_callback_enabled
"""
    $(SIGNATURES)

Returns *true* if the :cb:`Frame Rendered` callback is enabled, *false* otherwise.

.. versionadded:: 2.0.6\$nbsp;(Plugin)
"""
function is_frame_rendered_callback_enabled(device::BrickletLEDStrip)
    return device.deviceInternal.is_frame_rendered_callback_enabled()
end

export get_identity
"""
    $(SIGNATURES)

Returns the UID, the UID where the Bricklet is connected to,
the position, the hardware and firmware version as well as the
device identifier.

The position can be 'a', 'b', 'c', 'd', 'e', 'f', 'g' or 'h' (Bricklet Port).
A Bricklet connected to an :ref:`Isolator Bricklet <isolator_bricklet>` is always at
position 'z'.

The device identifier numbers can be found :ref:`here <device_identifier>`.
|device_identifier_constant|
"""
function get_identity(device::BrickletLEDStrip)
    return device.deviceInternal.get_identity()
end

export register_callback
"""
Registers the given *function* with the given *callback_id*.
"""
function register_callback(device::BrickletLEDStrip, callback_id, function_)
    if isnothing(function_)
        device.registered_callbacks.pop(callback_id, None)
    else
        device.registered_callbacks[callback_id] = function_
    end
end


text:
# The simple
struct Newton{T1,Tlin,Tage,TFact} <: QuasiNewton{T1}
    approx::T1
    linsolve::Tlin
    reset_age::Tage
    factorizer::TFact
end
hasprecon(::Newton) = NoPrecon()
# struct DefaultSequence end
default_newton_linsolve(B::Number, g) = B \ g
function default_newton_linsolve(B, g)
    B \ g
end
function default_newton_linsolve(d, B, g)
    d .= (B \ g)
end
Newton(; approx = Direct(), linsolve = default_newton_linsolve, reset_age = nothing) =
    Newton(approx, linsolve, reset_age, nothing)
summary(::Newton{<:Direct,typeof(default_newton_linsolve)}) =
    "Newton's method with default linsolve"
summary(::Newton{<:Direct,<:Any}) = "Newton's method with user-supplied linsolve"

max_stars_repo_name: JuliaNLSolvers/NLSolvers

max_issues_repo_name: JuliaNLSolvers/NLSolvers

max_forks_repo_name: JuliaNLSolvers/NLSolvers


text:
using Test
using Distributions

@testset "override pdf, cdf, etc." begin

    d = Distributions.Gamma(1.0, 2.0)

    μμ = rand(d, 50)
    σσ = rand(d, 50)

    for (μ, σ) in zip(μμ, σσ)
        l = Distributions.LogNormal(μ, σ)
        r = LRMoE.LogNormalExpert(μ, σ)
        x = rand(l, 100)
        @test LRMoE.logpdf(r, x) ≈ Distributions.logpdf.(l, x)
        @test LRMoE.logcdf(r, x) ≈ Distributions.logcdf.(l, x)
        @test LRMoE.pdf(r, x) ≈ Distributions.pdf.(l, x)
        @test LRMoE.cdf(r, x) ≈ Distributions.cdf.(l, x)
    end

end

max_stars_repo_name: sparktseung/JuliaLRMoE

max_issues_repo_name: sparktseung/JuliaLRMoE

max_forks_repo_name: sparktseung/JuliaLRMoE


text:
function Likelyhood_Ratio_test(simple,full)
    degrees = dof(full) - dof(simple)
    ccdf(Distributions.Chisq(degrees), deviance(simple) - deviance(full))
end

function AIC_test(candidate, simpler)
    exp((aic(candidate) - aic(simpler))/2)
end
function AICc(model)
    aic(model) + ((2*(dof(model)^2) + 2*dof(model))/(nobs(model) - dof(model) - 1))
end
function AICc_test(candidate, simpler)
    exp((AICc(candidate) - AICc(simpler))/2)
end


text:
using DenseSynchrony
using Documenter

makedocs(;
    modules=[DenseSynchrony],
    authors="anandijain <anandj@uchicago.edu> and contributors",
    repo="https://github.com/anandijain/DenseSynchrony.jl/blob/{commit}{path}#L{line}",
    sitename="DenseSynchrony.jl",
    format=Documenter.HTML(;
        prettyurls=get(ENV, "CI", "false") == "true",
        canonical="https://anandijain.github.io/DenseSynchrony.jl",
        assets=String[],
    ),
    pages=[
        "Home" => "index.md",
    ],
)

deploydocs(;
    repo="github.com/anandijain/DenseSynchrony.jl",
)


text:
export seed_glorot_normal, seed_glorot_uniform

import Flux: glorot_uniform, glorot_normal

using Random

glorot_uniform(rng::AbstractRNG, dims...) =
    (rand(rng, Float32, dims...) .- 0.5f0) .* sqrt(24.0f0 / sum(Flux.nfan(dims...)))
glorot_normal(rng::AbstractRNG, dims...) =
    randn(rng, Float32, dims...) .* sqrt(2.0f0 / sum(Flux.nfan(dims...)))

seed_glorot_uniform(; seed = nothing) =
    (dims...) -> glorot_uniform(MersenneTwister(seed), dims...)
seed_glorot_normal(; seed = nothing) =
    (dims...) -> glorot_normal(MersenneTwister(seed), dims...)


text:
# @with_kw struct D_Linear
#     #C_e.jl
#     D_ce::Function = z -> zeros(length(z))

#     #Phie.jl
#     D_ϕe_neg::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. (Lneg*(σ_eff_Neg/κ_eff_Neg)*(1-cosh(ν_neg*pt/Lneg)) - pt*ν_neg*sinh(ν_neg))/(CC_A*(κ_eff_Neg+σ_eff_Neg)*sinh(ν_neg)*ν_neg) + (Lneg*(cosh(ν_neg)-cosh(ν_neg*(Lneg-pt)/Lneg)/(CC_A*κ_eff_Neg*(κ_eff_Neg+σ_eff_Neg)*sinh(ν_neg)*ν_neg))) #Lee. Eqn. 4.22
#     D_ϕe_sep::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. (CellData.Neg.L - pt)/(CC_A*κ_eff_Sep) + (CellData.Neg.L*((1-σ_eff_Neg/κ_eff_Neg)*tanh(ν_neg/2)-ν_neg))/(CC_A*(κ_eff_Neg+σ_eff_Neg)*ν_neg) #Lee. Eqn. 4.23
#     D_ϕe_pos::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. -CellData.Sep.L/(CC_A*κ_eff_Sep) + (CellData.Neg.L*((1-σ_eff_Neg/κ_eff_Neg)*tanh(ν_neg/2)-ν_neg))/(CC_A*(κ_eff_Neg+σ_eff_Neg)*ν_neg) - (CellData.Pos.L*(1+(σ_eff_Pos/κ_eff_Pos)*cosh(ν_pos)) + CellData.Pos.L*cosh((CellData.Neg.L+CellData.Sep.L-pt)*ν_pos/CellData.Pos.L) + CellData.Pos.L*((σ_eff_Pos/κ_eff_Pos)*cosh((Ltot-pt)*ν_pos/CellData.Pos.L)))/(CC_A*(κ_eff_Pos+σ_eff_Pos)*sinh(ν_pos)*ν_pos) + (CellData.Neg.L + CellData.Sep.L - pt)/(CC_A*((κ_eff_Pos+σ_eff_Pos)))  #Lee. Eqn. 4.24

#     #Flux.jl
#     D_j::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. ν*(σ_eff*cosh(ν*z)+κ_eff*cosh(ν*(z-1)))/(as*F*Electrode.L*CC_A*(κ_eff+σ_eff)*sinh(ν))

#     #C_se.jl
#     D_cse::Function = z -> zeros(length(z))

#     #Phi_s.jl
#     D_ϕs::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. -Electrode.L*(κ_eff*(cosh(ν)-cosh(z-1)*ν))/(CellData.Const.CC_A*σ_eff*(comb_cond_eff)*ν*sinh(ν))-Electrode.L*(σ_eff*(1-cosh(z*ν)+z*ν*sinh(ν)))/(CellData.Const.CC_A*σ_eff*(comb_cond_eff)*ν*sinh(ν)) #Transfer Function - eq. 4.19

#     #Phi_se.jl
#     D_ϕse::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. Electrode.L/(CC_A*ν*sinh(ν))*((1/κ_eff)*cosh(ν*z)+(1/σ_eff)*cosh(ν*(z-1))) #Transfer Function - eq. 4.14

# end

max_stars_repo_name: BradyPlanden/RealisationAlgorithms

max_issues_repo_name: BradyPlanden/RealisationAlgorithms

max_forks_repo_name: BradyPlanden/RealisationAlgorithms


text:
using LinearAlgebra
function Base.exp(A::AbstractMatrix)
    T = Float64
    n = LinearAlgebra.checksquare(A)
    if ishermitian(A)
        return copytri!(parent(exp(Hermitian(A))), 'U', true)
    end
    #ilo, ihi, scale = LAPACK.gebal!('B', A)    # modifies A
    nA   = opnorm(A, 1)
    Inn    = Matrix{T}(I, n, n)
    ## For sufficiently small nA, use lower order Padé-Approximations
    if (nA <= 2.1)
        if nA > 0.95
            C = T[17643225600.,8821612800.,2075673600.,302702400.,
                     30270240.,   2162160.,    110880.,     3960.,
                           90.,         1.]
        elseif nA > 0.25
            C = T[17297280.,8648640.,1995840.,277200.,
                     25200.,   1512.,     56.,     1.]
        elseif nA > 0.015
            C = T[30240.,15120.,3360.,
                    420.,   30.,   1.]
        else
            C = T[120.,60.,12.,1.]
        end
        A2 = A * A
        P  = copy(Inn)
        U  = C[2] * P
        V  = C[1] * P
        for k in 1:(div(size(C, 1), 2) - 1)
            k2 = 2 * k
            P *= A2
            U += C[k2 + 2] * P
            V += C[k2 + 1] * P
        end
        U = A * U
        X = V + U
        #LAPACK.gesv!(V-U, X)
        X = (V-U)\X
    else
        s  = log2(nA/5.4)               # power of 2 later reversed by squaring
        if s > 0
            si = ceil(Int,s)
            A /= convert(T,2^si)
        end
        CC = T[64764752532480000.,32382376266240000.,7771770303897600.,
                1187353796428800.,  129060195264000.,  10559470521600.,
                    670442572800.,      33522128640.,      1323241920.,
                        40840800.,           960960.,           16380.,
                             182.,                1.]
        A2 = A * A
        A4 = A2 * A2
        A6 = A2 * A4
        U  = A * (A6 * (CC[14].*A6 .+ CC[12].*A4 .+ CC[10].*A2) .+
                  CC[8].*A6 .+ CC[6].*A4 .+ CC[4].*A2 .+ CC[2].*Inn)
        V  = A6 * (CC[13].*A6 .+ CC[11].*A4 .+ CC[9].*A2) .+
                   CC[7].*A6 .+ CC[5].*A4 .+ CC[3].*A2 .+ CC[1].*Inn

        X = V + U
        #LAPACK.gesv!(V-U, X)
        X = (V-U)\X

        if s > 0            # squaring to reverse dividing by power of 2
            for t=1:si; X *= X end
        end
    end

    # # Undo the balancing
    # for j = ilo:ihi
    #     scj = scale[j]
    #     for i = 1:n
    #         X[j,i] *= scj
    #     end
    #     for i = 1:n
    #         X[i,j] /= scj
    #     end
    # end

    # if ilo > 1       # apply lower permutations in reverse order
    #     for j in (ilo-1):-1:1; rcswap!(j, Int(scale[j]), X) end
    # end
    # if ihi < n       # apply upper permutations in forward order
    #     for j in (ihi+1):n;    rcswap!(j, Int(scale[j]), X) end
    # end
    X
end

max_stars_repo_name: hpigot/windkessel-id

max_issues_repo_name: hpigot/windkessel-id

max_forks_repo_name: hpigot/windkessel-id