Fix up initializesystem for hierarchical models

The first version only worked on non-hierarchical models. Now it handles all models. Here's some example usage. Here's an example model:

```julia
using ModelingToolkit, DifferentialEquations

@parameters t
D = Differential(t)

@connector Port begin
    p(t)
    dm(t)=0, [connect = Flow]
end

@connector Flange begin
    dx(t)=0
    f(t), [connect = Flow]
end

# Components ----
@mtkmodel Orifice begin
    @parameters begin
        Cₒ=2.7
        Aₒ=0.00094
        ρ₀=1000
        p′=0
    end
    @variables begin
        dm(t)=0
        p₁(t)=p′
        p₂(t)=p′
    end
    @components begin
        port₁ = Port(p=p′)
        port₂ = Port(p=p′)
    end
    begin
        u = dm/(ρ₀*Aₒ)
    end
    @equations begin
        dm ~ +port₁.dm
        dm ~ -port₂.dm
        p₁ ~ port₁.p
        p₂ ~ port₂.p

        p₁ - p₂ ~ (1/2)*ρ₀*u^2*Cₒ
    end
end

@mtkmodel Volume begin
    @parameters begin
        A=0.1
        ρ₀=1000
        β=2e9
        direction=+1
        p′
        x′
    end
    @variables begin
        p(t)=p′
        x(t)=x′
        dm(t)=0
        f(t)=p′ * A
        dx(t)=0
        r(t), [guess = 1000]
        dr(t), [guess = 1000]
    end
    @components begin
        port = Port(p=p′)
        flange = Flange(f=-p′ * A * direction)
    end
    @equations begin
        D(x) ~ dx
        D(r) ~ dr

        p ~ +port.p
        dm ~ +port.dm # mass is entering
        f ~ -flange.f * direction # force is leaving
        dx ~ flange.dx * direction

        r ~ ρ₀*(1 + p/β)
        dm ~ (r*dx*A) + (dr*x*A)
        f ~ p * A
    end
end

@mtkmodel Mass begin
    @parameters begin
        m = 100
        f′
    end
    @variables begin
        f(t)=f′
        x(t)=0
        dx(t)=0
        ẍ(t)=f′/m
    end
    @components begin
        flange = Flange(f=f′)
    end
    @equations begin
        D(x) ~ dx
        D(dx) ~ ẍ

        f ~ flange.f
        dx ~ flange.dx

        m*ẍ ~ f
    end
end

@mtkmodel Actuator begin
    @parameters begin
        p₁′
        p₂′
    end
    begin #constants
        x′=0.5
        A=0.1
    end
    @components begin
        port₁ = Port(p=p₁′)
        port₂ = Port(p=p₂′)
        vol₁ = Volume(p′=p₁′, x′=x′,  direction=-1)
        vol₂ = Volume(p′=p₂′, x′=x′,  direction=+1)
        mass = Mass(f′=(p₂′ - p₁′)*A)
        flange = Flange(f=0)
    end
    @equations begin
        connect(port₁, vol₁.port)
        connect(port₂, vol₂.port)
        connect(vol₁.flange, vol₂.flange, mass.flange, flange)
    end
end

@mtkmodel Source begin
    @parameters begin
        p′
    end
    @components begin
        port = Port(p=p′)
    end
    @equations begin
        port.p ~ p′
    end
end

@mtkmodel Damper begin
    @parameters begin
        c = 1000
    end
    @components begin
        flange = Flange(f=0)
    end
    @equations begin
        flange.f ~ c*flange.dx
    end
end

@mtkmodel System begin
    @components begin
        res₁ = Orifice(p′=300e5)
        res₂ = Orifice(p′=0)
        act = Actuator(p₁′=300e5, p₂′=0)
        src = Source(p′=300e5)
        snk = Source(p′=0)
        dmp = Damper()
    end
    @equations begin
        connect(src.port, res₁.port₁)
        connect(res₁.port₂, act.port₁)
        connect(act.port₂, res₂.port₁)
        connect(res₂.port₂, snk.port)
        connect(dmp.flange, act.flange)
    end
end

@mtkbuild sys = System()
```

It's having troubles initializing, so what is the system it's trying to initialize?

```julia
initsys = initializesystem(sys)
```

That gives it symbolically. We have 98 equations for 54 variables, shesh that's overloaded.

```julia
julia> initprob = NonlinearProblem(initsys,[])
ERROR: ArgumentError: Equations (98), states (54), and initial conditions (54) are of different lengths. To allow a different number of equations than states use kwarg check_length=false.
Stacktrace:
 [1] check_eqs_u0(eqs::Vector{…}, dvs::Vector{…}, u0::Vector{…}; check_length::Bool, kwargs::@kwargs{})
   @ ModelingToolkit C:\Users\accou\.julia\packages\ModelingToolkit\Gpzyo\src\systems\abstractsystem.jl:1871
 [2] process_NonlinearProblem(constructor::Type, sys::NonlinearSystem, u0map::Vector{…}, parammap::SciMLBase.NullParameters; version::Nothing, jac::Bool, checkbounds::Bool, sparse::Bool, simplify::Bool, linenumbers::Bool, parallel::Symbolics.SerialForm, eval_expression::Bool, use_union::Bool, tofloat::Bool, kwargs::@kwargs{…})
   @ ModelingToolkit C:\Users\accou\.julia\packages\ModelingToolkit\Gpzyo\src\systems\nonlinear\nonlinearsystem.jl:339
 [3] process_NonlinearProblem
   @ C:\Users\accou\.julia\packages\ModelingToolkit\Gpzyo\src\systems\nonlinear\nonlinearsystem.jl:323 [inlined]
 [4] (NonlinearProblem{…})(sys::NonlinearSystem, u0map::Vector{…}, parammap::SciMLBase.NullParameters; check_length::Bool, kwargs::@kwargs{})
   @ ModelingToolkit C:\Users\accou\.julia\packages\ModelingToolkit\Gpzyo\src\systems\nonlinear\nonlinearsystem.jl:368
 [5] NonlinearProblem (repeats 2 times)
   @ ModelingToolkit C:\Users\accou\.julia\packages\ModelingToolkit\Gpzyo\src\systems\nonlinear\nonlinearsystem.jl:365 [inlined]
 [6] #NonlinearProblem#698
   @ ModelingToolkit C:\Users\accou\.julia\packages\ModelingToolkit\Gpzyo\src\systems\nonlinear\nonlinearsystem.jl:362 [inlined]
 [7] NonlinearProblem(sys::NonlinearSystem, args::Vector{Any})
   @ ModelingToolkit C:\Users\accou\.julia\packages\ModelingToolkit\Gpzyo\src\systems\nonlinear\nonlinearsystem.jl:361
 [8] top-level scope
   @ REPL[1]:1
Some type information was truncated. Use `show(err)` to see complete types.
```

So we can't build a NonlinearProblem. But we can do this:

```julia
initprob = NonlinearProblem(initsys,[], check_length=false, checkbounds = true)
lsqprob = NonlinearLeastSquaresProblem(NonlinearFunction(initprob.f.f, resid_prototype = zeros(98)), initprob.u0, initprob.p)
initsol = solve(lsqprob, GaussNewton(), reltol = 1e-12, abstol = 1e-12)
retcode: Success
u: 54-element Vector{Float64}:
      0.5
   1015.0
      0.5
   1000.0
      0.0
     -1.208682818218319e-28
      ⋮
      5.048709793414476e-28
     -3.52499105861307e-29
     -1.5146129380243427e-28
     -3.0e6
 -30000.0
     -3.0e6
```

And now we can make our initial conditions match that:

```julia
allinit = states(initsys) .=> initsol
prob = ODEProblem(sys, allinit, (0,0.1))
```

and solve:

```julia
sol = solve(prob)
tmp = [0.0, 0.0, -0.0, -0.0, -0.0, -0.0, 8.123585990009498e-54, 1.1599794698483246e-51, 1.5375323209568473e-26, -2.914685306392616e-26]
┌ Warning: Instability detected. Aborting
└ @ SciMLBase C:\Users\accou\.julia\packages\SciMLBase\3Mujd\src\integrator_interface.jl:602
```

Here is modified the solver so tmp is the `resid` vector inside of the initializer. You can see it worked because the initialization errors are all zero. This is as opposed to with the default initial conditions:

```julia
sysguesses = [ModelingToolkit.getguess(st) for st in states(sys)]
hasaguess = findall(!isnothing, sysguesses)
sysguesses = Dict(states(sys)[hasaguess] .=> sysguesses[hasaguess])
prob = ODEProblem(sys, sysguesses, (0,0.1))
sol = solve(prob)
```

```julia
julia> sol = solve(prob)
tmp = [-0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3.0e7, 0.0, 50.0, 50.0]
nlsol.resid = [-3.0e7, 0.0, 50.0, 50.0]
nlsol.retcode = SciMLBase.ReturnCode.MaxIters
retcode: InitialFailure
Interpolation: specialized 4rd order "free" stiffness-aware interpolation
t: 1-element Vector{Float64}:
 0.0
u: 1-element Vector{Vector{Float64}}:
 [0.5, 1000.0, 0.5, 1000.0, 0.0, 0.0, 0.0, 0.0, 1000.0, 1000.0]
```

Author: ChrisRackauckas

src/systems/nonlinear/initializesystem.jl

@@ -3,51 +3,51 @@ $(TYPEDSIGNATURES)
 
 Generate `NonlinearSystem` which initializes an ODE problem from specified initial conditions of an `ODESystem`.
 """
-function initializesystem(sys::ODESystem; name = nameof(sys), kwargs...)
-    if has_parent(sys) && (parent = get_parent(sys); parent !== nothing)
-        sys = parent
-    end
+function initializesystem(sys::ODESystem; name = nameof(sys), guesses = Dict(), kwargs...)
     sts, eqs = unknowns(sys), equations(sys)
-
     idxs_diff = isdiffeq.(eqs)
     idxs_alge = .!idxs_diff
-
-    # Algebraic equations and initial guesses are unchanged
-    eqs_ics = similar(eqs)
-    u0 = Vector{Any}(undef, length(sts))
-
-    eqs_ics[idxs_alge] .= eqs[idxs_alge]
-    u0[idxs_alge] .= getmetadata.(unwrap.(sts[idxs_alge]),
-        Symbolics.VariableDefaultValue,
-        nothing)
-
-    for idx in findall(idxs_diff)
-        st = sts[idx]
-        if !hasdefault(st)
-            error("Invalid setup: unknown $(st) has no default value or equation.")
-        end
-
-        def = getdefault(st)
-        if def isa Equation
-            if !hasguess(st)
-                error("Invalid setup: unknown $(st) has an initial condition equation with no guess.")
+    num_alge = sum(idxs_alge)
+
+    # Start the equations list with algebraic equations
+    eqs_ics = eqs[idxs_alge]
+    u0 = Vector{Pair}(undef, 0)
+    defs = ModelingToolkit.defaults(sys)
+
+    full_states = [sts;getfield.((observed(sys)),:lhs)]
+
+    # Refactor to ODESystem construction
+    # should be ModelingToolkit.guesses(sys)
+    sysguesses = [ModelingToolkit.getguess(st) for st in full_states]
+    hasaguess = findall(!isnothing, sysguesses)
+    sysguesses = todict(full_states[hasaguess] .=> sysguesses[hasaguess])
+    guesses = merge(sysguesses, todict(guesses))
+
+    for st in full_states
+        if st ∈ keys(defs)
+            def = defs[st]
+
+            if def isa Equation
+                st ∉ keys(guesses) && error("Invalid setup: unknown $(st) has an initial condition equation with no guess.")
+                push!(eqs_ics,def)
+                push!(u0,st => guesses[st])
+            else
+                push!(eqs_ics,st ~ def)
+                push!(u0, st => def)
             end
-            guess = getguess(st)
-            eqs_ics[idx] = def
-
-            u0[idx] = guess
+        elseif st ∈ keys(guesses)
+            push!(u0,st => guesses[st])
         else
-            eqs_ics[idx] = st ~ def
-
-            u0[idx] = def
+            error("Invalid setup: unknown $(st) has no default value or initial guess")
         end
     end
 
     pars = parameters(sys)
-    sys_nl = NonlinearSystem(eqs_ics,
-        sts,
+
+    sys_nl = NonlinearSystem([eqs_ics; observed(sys)],
+        full_states,
         pars;
-        defaults = Dict(sts .=> u0),
+        defaults = merge(ModelingToolkit.defaults(sys),todict(u0)),
         name,
         kwargs...)