# Inertial Brownian motion simulation

The Inertial Langevin equation for a particle of mass $m$ and some damping $\gamma$ writes:

\begin{equation}
m\ddot{x} = -\gamma \dot{x} + \sqrt{2k_\mathrm{B}T \gamma} \mathrm{d}B_t
\end{equation}

Integrating the latter equation using the Euler method, one can replace $\dot{x}$ by:

\begin{equation}
\dot{x} \simeq \frac{x_i - x_{i-1}}{\tau} ~,
\end{equation}

$\ddot{x}$ by:

\begin{equation}
	\begin{aligned}
	\ddot{x} &\simeq
	\frac{
		\frac{x_i - x_{i-1}}{\tau}
		-
		\frac{x_{i-1} - x_{i-2}}{\tau}
	}
	{\tau} \\
	& =  \frac{x_i - 2x_{i - 1} + x_{i-2}}{\tau^2} ~.
	\end{aligned}
\end{equation}

and finally, $\mathrm{d}B_t$ by a Gaussian random number $w_i$ with a zero mean value and a $\tau$ variance, on can write $x_i$ as:

\begin{equation}
	x_i = \frac{2 + \tau /\tau_\mathrm{B}}{1 + \tau / \tau_\mathrm{B} } x_{i-1}
	- \frac{1}{1 + \tau / \tau_\mathrm{B}}x_{i-2}
	+ \frac{\sqrt{2k_\mathrm{B}T\gamma}}{m(1 + \tau/\tau_\mathrm{B})} \tau w_i ~,
\end{equation}

In the following, we use Python to simulate such a movement and check the properties of the mean squared displacement. Then, I propose a Cython implementation that permits a $200$x speed improvement on the simulation.

```python
# Import important libraries
import numpy as np
import matplotlib.pyplot as plt

```

```python
# Just some matplotlib tweaks
import matplotlib as mpl

mpl.rcParams["xtick.direction"] = "in"
mpl.rcParams["ytick.direction"] = "in"
mpl.rcParams["lines.markeredgecolor"] = "k"
mpl.rcParams["lines.markeredgewidth"] = 1.5
mpl.rcParams["figure.dpi"] = 200
from matplotlib import rc

rc("font", family="serif")
rc("text", usetex=True)
rc("xtick", labelsize="medium")
rc("ytick", labelsize="medium")
rc("axes", labelsize="large")

def cm2inch(value):
    return value / 2.54
```

```python
N = 1000000  # number of time steps
tau = 0.01  # simulation time step
m = 1e-8  # particle mass
a = 1e-6  # radius of the particle
eta = 0.001  # viscosity (here water)
gamma = 6 * np.pi * eta * a
kbT = 4e-21
tauB = m / gamma
```

With such properties we have a characteristic diffusion time $\tau_\mathrm{B} =0.53$ s.

```python
def xi(xi1, xi2):
    """
    Function that compute the position of a particle using the full Langevin Equation
    """
    t = tau / tauB
    wi = np.random.normal(0, np.sqrt(tau))
    return (
        (2 + t) / (1 + t) * xi1
        - 1 / (1 + t) * xi2
        + np.sqrt(2 * kbT * gamma) / (m * (1 + t)) * np.power(tau,1) * wi
    )

```

```python
def trajectory(N):
    """
    Function generating a trajectory of length N.
    """
    x = np.zeros(N)
    for i in range(2, len(x)):
        x[i] = xi(x[i - 1], x[i - 2])
    return x

```

Now that the functions are setup one can simply generate a trajectory of length $N$ by simply calling the the function ```trajectory()```

```python
# Generate a trajectory of 10e6 points.
x = trajectory(1000000)
```

```python
plt.plot(np.arange(len(x))*tau, x)
plt.title("Intertial Brownian trajectory")
plt.ylabel("$x$ (m)")
plt.xlabel("$t$ (s)")
plt.show()
```

## Cross checking

We now check that the simulated trajectory gives us the correct MSD properties to ensure the simulation si done properly. The MSD given by:

\begin{equation}
\mathrm{MSD}(\Delta t) = \left. \langle \left( x(t) - x(t+\Delta t \right)^2 \rangle \right|_t ~,
\end{equation}

with $\Delta t$ a lag time. The MSD, can be computed using the function defined in the cell below. For a lag time $\Delta t \ll \tau_B$ we should have:

\begin{equation}
\mathrm{MSD}(\Delta t) = \frac{k_\mathrm{B}T}{m} \Delta t ^2 ~,
\end{equation}

and for $\Delta t \gg \tau_B$:

\begin{equation}
\mathrm{MSD}(\tau) = 2 D \Delta t~,
\end{equation}

with $D = k_\mathrm{B}T / (6 \pi \eta a)$.

```python
t = np.array([*np.arange(1,10,1), *np.arange(10,100,10), *np.arange(100,1000,100), *np.arange(1000,10000,1000)])
def msd(x,Dt):
    """Function that return the MSD for a list of time index t for a trajectory x"""
    _msd = lambda x, t : np.mean((x[:-Dt] - x[Dt:])**2)
    return [_msd(x,i) for i in t]
MSD = msd(x,t)
```

```python
D = kbT/(6*np.pi*eta*a)
t_plot = t*tau
plt.loglog(t*tau,MSD, "o")
plt.plot(t*tau, (2*D*t_plot), "--", color = "k", label="long time theory")
plt.plot(t*tau, kbT/m * t_plot**2, ":", color = "k", label="short time theory")

horiz_data = [1e-8, 1e-17]
t_horiz = [tauB, tauB]
plt.plot(t_horiz, horiz_data, "k", label="$\\tau_\mathrm{B}$")
plt.legend()
plt.show()
```

The simulations gives expected results. However, with the computer used, 6 seconds are needed to generate this trajectory. If someone wants to look at fine effects and need to generate millions of trajectories it is too long. In order to fasten the process, in the following  I use Cython to generate the trajectory using C language.

## Cython acceleration

```python
# Loading Cython library
%load_ext Cython
```

We now write the same functions as in the first part of the appendix. However, we now indicate the type of each variable.

```cython
%%cython
import cython
cimport numpy as np
import numpy as np
from libc.math cimport sqrt
ctypedef np.float64_t dtype_t

cdef int N = 1000000 # length of the simulation

cdef dtype_t tau = 0.01 # simulation time step
cdef dtype_t m = 1e-8 # particle mass
cdef dtype_t a = 1e-6 # radius of the particle
cdef dtype_t eta = 0.001 # viscosity (here water)
cdef dtype_t gamma = 6 * 3.14 * eta * a
cdef dtype_t kbT = 4e-21
cdef dtype_t tauB = m/gamma
cdef dtype_t[:] x = np.zeros(N)

@cython.boundscheck(False)
@cython.wraparound(False)
@cython.nonecheck(False)
@cython.cdivision(True)
cdef dtype_t xi_cython( dtype_t xi1, dtype_t xi2, dtype_t wi):
    cdef dtype_t t = tau / tauB
    return (
        (2 + t) / (1 + t) * xi1
        - 1 / (1 + t) * xi2
        + sqrt(2 * kbT * gamma) / (m * (1 + t)) * tau * wi
    )

@cython.boundscheck(False)
@cython.wraparound(False)
@cython.nonecheck(False)
cdef dtype_t[:] _traj(dtype_t[:] x, dtype_t[:] wi):
    cdef int i
    for i in range(2, N):

        x[i] = xi_cython(x[i-1], x[i-2], wi[i])
    return x

def trajectory_cython():

    cdef dtype_t[:] wi = np.random.normal(0, np.sqrt(tau), N).astype('float64')

    return _traj(x, wi)
```

```python
%timeit trajectory_cython()
```

    28.7 ms ± 376 µs per loop (mean ± std. dev. of 7 runs, 10 loops each)

Again, we check that the results given through the use of Cython gives the correct MSD

```python
x=np.asarray(trajectory_cython())
D = kbT/(6*np.pi*eta*a)
t_plot = t*tau
plt.loglog(t*tau,MSD, "o")
plt.plot(t*tau, (2*D*t_plot), "--", color = "k", label="long time theory")
plt.plot(t*tau, kbT/m * t_plot**2, ":", color = "k", label="short time theory")

horiz_data = [1e-8, 1e-17]
t_horiz = [tauB, tauB]
plt.plot(t_horiz, horiz_data, "k", label="$\\tau_\mathrm{B}$")
plt.xlabel("$\Delta t$")
plt.ylabel("$MSD (m$^2$)$")
plt.legend()
plt.show()
```

### Conclusion

Finally, one only needs $\simeq 30$ ms to generate the trajectory instead of $\simeq 6s$ which is a
$\simeq 200\times$ improvement speed. The simulation si here bound to the time needed to generate the array of random numbers which is still done using numpy function. After further checking, Numpy random generation si as optimize as one could do so there is no benefit on cythonizing the random generation. For the sake of completness one could fine a Cython version to generate random numbers. Found thanks to Senderle: https://stackoverflow.com/questions/42767816/what-is-the-most-efficient-and-portable-way-to-generate-gaussian-random-numbers

```cython
%%cython
from libc.stdlib cimport rand, RAND_MAX
from libc.math cimport log, sqrt
import numpy as np
import cython

cdef double random_uniform():
    cdef double r = rand()
    return r / RAND_MAX

cdef double random_gaussian():
    cdef double x1, x2, w

    w = 2.0
    while (w >= 1.0):
        x1 = 2.0 * random_uniform() - 1.0
        x2 = 2.0 * random_uniform() - 1.0
        w = x1 * x1 + x2 * x2

    w = ((-2.0 * log(w)) / w) ** 0.5
    return x1 * w

@cython.boundscheck(False)
cdef void assign_random_gaussian_pair(double[:] out, int assign_ix):
    cdef double x1, x2, w

    w = 2.0
    while (w >= 1.0):
        x1 = 2.0 * random_uniform() - 1.0
        x2 = 2.0 * random_uniform() - 1.0
        w = x1 * x1 + x2 * x2

    w = sqrt((-2.0 * log(w)) / w)
    out[assign_ix] = x1 * w
    out[assign_ix + 1] = x2 * w

@cython.boundscheck(False)
def my_uniform(int n):
    cdef int i
    cdef double[:] result = np.zeros(n, dtype='f8', order='C')
    for i in range(n):
        result[i] = random_uniform()
    return result

@cython.boundscheck(False)
def my_gaussian(int n):
    cdef int i
    cdef double[:] result = np.zeros(n, dtype='f8', order='C')
    for i in range(n):
        result[i] = random_gaussian()
    return result

@cython.boundscheck(False)
def my_gaussian_fast(int n):
    cdef int i
    cdef double[:] result = np.zeros(n, dtype='f8', order='C')
    for i in range(n // 2):  # Int division ensures trailing index if n is odd.
        assign_random_gaussian_pair(result, i * 2)
    if n % 2 == 1:
        result[n - 1] = random_gaussian()

    return result

```

```python
%timeit my_gaussian_fast(1000000)
```

```python
%timeit np.random.normal(0,1,1000000)
```

One can thus see, that even a pure C implementation can be slower than the Numpy one, thanks to a great optimization.

```python
fig = plt.figure(figsize = (cm2inch(16), cm2inch(10)))
gs = fig.add_gridspec(2, 1)
f_ax1 = fig.add_subplot(gs[0, 0])
for i in range(100):
    x = np.asarray(trajectory_cython())* 1e6
    plt.plot(np.arange(N)*tau / 60, x)

plt.ylabel("$x$ ($\mathrm{\mu m}$)")
plt.xlabel("time (minutes)")
plt.text(5,100, "a)")
plt.xlim([0,None])
f_ax1 = fig.add_subplot(gs[1, 0])

x=np.asarray(trajectory_cython())
D = kbT/(6*np.pi*eta*a)
t_plot = t*tau
plt.loglog(t*tau,MSD, "o")
plt.plot(t*tau, (2*D*t_plot), "--", color = "k", label="long time theory")
plt.plot(t*tau, kbT/m * t_plot**2, ":", color = "k", label="short time theory")

horiz_data = [1e-8, 1e-17]
t_horiz = [tauB, tauB]
plt.plot(t_horiz, horiz_data, "k", label="$\\tau_\mathrm{B}$")
plt.ylabel("MSD (m$^2$)")
plt.xlabel("$\\Delta t$ (s)")
ax = plt.gca()
locmaj = mpl.ticker.LogLocator(base=10.0, subs=(1.0, ), numticks=100)
ax.yaxis.set_major_locator(locmaj)
locmin = mpl.ticker.LogLocator(base=10.0, subs=np.arange(2, 10) * .1,
                                      numticks=100)
ax.yaxis.set_minor_locator(locmin)
ax.yaxis.set_minor_formatter(mpl.ticker.NullFormatter())
plt.legend(frameon=False)
plt.text(1e2,1e-16, "b)")

plt.tight_layout(pad=0.4, w_pad=0.5, h_pad=1.0)

plt.savefig("intertial_langevin.pdf")
```


# Tutorial 2 - Two tanks system

## Background information
This tutorial shows how to use _numerous_ to create a system of multiple components.
The tutorial is aimed at demonstrating the usability and scalability of the model architecture for systems with multiple physical components connected bewteen each other by means of connectors.

The tutorial is built based on a relative simple system, e.g. two tanks are placed on top of each other and connected by a valve. The implementation using _numerous_ might therefore seems tedious and over-complicated, given the limited number of components and thus equations to solve. However, as already pintpointed, we wish to illustrate how systems can be created, and how _numerous_ model architecture would be advantegeous for systems with a high degree of complexity.

The model implementation is developed by instantiating _items_ of different classes, and connecting them by means of _connectors_.

The tutorial can be run only importing the extra packages pandas, numpy and plotly.

## System governing equations

A system of two tanks and a valve is shown in the figure below. H indicates the height of a certain mass M of liquid in each tank. The liquid density is indicated by $\rho$, while the cross-sectional area of each tank is indicated by the parameter $A_0$.The mass flow rate through the valve is determined by a valve characteristic parameter  $\mathrm{C_v}$.

At time 0, the top tank (tank 1) is filled with a certain amount of liquid, determining the height $\mathrm{H_1}$, while the bottom tank (tank 2) is completely empty. If the valve is opened at time 0, a mass flow rate starts flowing through the valve between tank 1 and tank 2. After a given time (that we indicate as $t_\mathrm{filling}$) the top tank will be empty, while the bottom tank will contain the entire mass of liquiq.

The mathematical model describing this systme is illusrated below.

For the sake of simplicity, we assume that the mass flow rate through the valve is proportional to the pressure drop accross the valve by the relation:
$$\begin{equation}
\dot{\mathrm{m}} = \mathrm{C_v} \sqrt{\mathrm{\Delta p}}
\end{equation}
$$

Moreover, the conservation of mass in the two tanks is expressed by the equations (with the assumption of incompressible fluid):
$$\begin{equation}
\rho A_{0,1} \mathrm{\dfrac{dH_1}{dt}} = - \dot{m}
\end{equation}
$$
$$\begin{equation}
\rho A_{0,2}  \mathrm{\dfrac{dH_2}{dt}} = + \dot{m}
\end{equation}
$$

The pressure drop accross the valve can be expressed by the equation:
$$\begin{equation}
\Delta p = \rho g H_1
\end{equation}
$$

By substituting this latter expression in the previous equations, the differential system of equations describing the system becomes:
$$\begin{equation}
\rho A_{0,1} \mathrm{\dfrac{dH_1}{dt}} = - \mathrm{C_v} (t)\sqrt{\rho g \mathrm{H_1}}
\end{equation}
$$
$$\begin{equation}
\rho A_{0,2} \mathrm{\dfrac{dH_2}{dt}} = + \mathrm{C_v}(t)\sqrt{\rho g \mathrm{H_1}}
\end{equation}
$$

## Analytical solution
The system of differential equations above can be quite complex to solve analytically given the time dependence of $\mathrm{C_v}$. Therefore, a numerical solver can be used to solve the system, and we will show in this tutorial how to create a model and to solve it using _numerous_.

However, in order to prove the correct implementation of the solver, we will compare the results of the numerical solution against the analytical solution for the simplyfing case of having a characteristic valve parameter $\mathrm{C_v}$ independent from time.

If we equal the parameter to a constant value over time  $\mathrm{C_v}(t) = \mathrm{C_v}$, it is possible to solve the system analytically. In fact, the integration of the previous equations leads to the solution:
$$\begin{equation}
\mathrm{H_1(t)} = \Bigg[\sqrt{\mathrm{H_{1,0}}} - \dfrac{\mathrm{C_v}}{2 A_{0,1}} \sqrt{\dfrac{g}{\rho}} \cdot t \Bigg]^{2}
\end{equation}
$$
$$\begin{equation}
\mathrm{H_2(t)} = \mathrm{H_{2,0}} + \dfrac{\mathrm{C_v}}{A_{0,2}} \sqrt{ \dfrac{g}{\rho}} \sqrt{\mathrm{H_{1,0}}} \cdot t - \dfrac{\mathrm{C_v^{2}}}{4 A_{0,1} A_{0,2}} \dfrac{g}{\rho} \cdot t^2
\end{equation}
$$

The solution can also be expressed as function of the total mass in the tanks, which is related to the liquid height by the volume as:
$$\begin{equation}
\mathrm{M} = \rho \mathrm{H} \mathrm{A_0}
\end{equation}
$$

The analytical solution would thus be expressed as:
$$\begin{equation}
\mathrm{M_1(t)} = \Bigg[\sqrt{\rho \mathrm{H_{1,0} A_{0,1}}} - \dfrac{\mathrm{C_v}}{2} \sqrt{\dfrac{g}{A_{0,1}}} \cdot  t \Bigg]^{2}
\end{equation}
$$
$$\begin{equation}
\mathrm{M_2(t)} = \mathrm{\rho H_{2,0}} A_{0,2}  + \mathrm{C_v} \sqrt{ \dfrac{g}{A_{0,1}}} \sqrt{\mathrm{\rho \mathrm{H_{1,0} A_{0,1}}}} \cdot t - \dfrac{\mathrm{C_v^{2}}}{4} \dfrac{g}{A_{0,1}} \cdot  t^2
\end{equation}
$$

## Input data
The input data used for the analytical solution and the simulation are reported below:
* liquid density, $\rho$ = 1000 kg/m$^3$
* cross-sectional area tank 1, $A_{0,1}$ = 0.05 m$^2$
* cross-sectional area tank 2, $A_{0,2}$ = 0.1 m$^2$
* initial conditions:
    * initial height tank 1  $H_{0,1}$ = 0.5 m
    * initial height tank 2  $H_{0,2}$ = 0.0 m

The implementation of the model with a time-dependent $\mathrm{C_v}$ was carried out by using the following formulation:
* $\mathrm{C_v} = \mathrm{C_{v,0}} \cdot \big[sin{(\omega \cdot t)}+1\big]  $
and imposing:
    * $\mathrm{C_{v,0}} = 0.1 \; \; \; (\mathrm{kg}/\mathrm{s})/\mathrm{Pa}$
    * $\omega \; \; \;  = 1.5 \; \; \;\mathrm{rad/s}$

A constant value of the valve parameter $\mathrm{C_v}$ was instead obtained by imposing $\omega = 0$, so that:
* $\mathrm{C_v} = \mathrm{C_{v,0}}$

at any time step, and the solution could be compared against the analytical formulation derived above.

This is translated into the followinf python code:

### Definition of input data - code:

```python
import numpy as np
## Input data
# Define liquid and geometry for the case in analysis
rho_water = 1000           # Water density [kg/m3]
A0        = [0.05, 0.1]    # Tanks cross-sectional area [m2]
Cv_0      = 0.1            # [(kg s)/Pa] Amplitude of valve characteristic parameter
g         = 9.81           # [m/s2] gravitational accelleration

## Initial conditions
H0        = [0.5, 0]     # Initial condition - liquid height [m]
# Estimate the initial value of total mass M [kg] in the two tanks
M0        = rho_water * np.multiply(A0, H0)

# Caclulate what is the time (analytical solution) to entirely fill the tank (simple case - analytical sol)
t_filling = np.sqrt(A0[0]/g) * np.sqrt(M0[0]) *2* 1/Cv_0

# Define start and stop time for the analysis and simulation
t_start_sim = 0
t_stop_sim  = 10
```

### Analytical soltion: Python code
First of all, we define a function which is able to return the analytical solution for a given time span, given the input parameters. The implementation below is based on the analytical solution derived above.

```python
import pandas as pd

N_t             = 11                           # Number of time steps at which the analytical solution is evaluated
time_range      = [t_start_sim ,t_stop_sim]    # Time span in which the solution is plotted

def analytical_solution(N_t=N_t, time_range=time_range, g=9.81,  A0=[0.05, 0.1], Cv_0=0.1,
                         M0=[25, 0], H0=[0.5, 0] , rho_water=1000):
    time_vector       = np.linspace(start = time_range[0], stop = time_range[1], num = N_t)

    # Create a dictionary with time, evolution of liquid heights and mass in the two tanks for the analytical solution:
    result_analytical = {'t': [], 'M_1': [], 'M_2': [], 'H_1': [], 'H_2': []}

    # Calculate what is the time after which the first tank is empty and the second tank is full
    t_filling = np.sqrt(A0[0]/g) * np.sqrt(M0[0]) *2* 1/Cv_0

    for i in range(N_t):
        if time_vector[i]  < t_filling:
            M_1_analytical = (np.sqrt(M0[0]) - Cv_0 / 2 * np.sqrt(g / A0[0]) * time_vector[i] ) ** 2
            M_2_analytical = np.sqrt(M0[1]) + Cv_0 * np.sqrt(g / A0[0]) * np.sqrt(
                M0[0]) * time_vector[i]  - Cv_0 ** 2 * g / A0[0] / 4 * (time_vector[i]  ** 2)

            H_1_analytical = (np.sqrt(H0[0]) - Cv_0 / 2 /A0[0] * np.sqrt(g/ rho_water) * time_vector[i] ) ** 2
            H_2_analytical = np.sqrt(H0[1]) + Cv_0 /A0[1] * np.sqrt(g/ rho_water) * np.sqrt(
                H0[0]) * time_vector[i]  - Cv_0 ** 2 * g / A0[0] / 4 /A0[1]/rho_water* (time_vector[i]  ** 2)
        else:
            M_2_analytical = M0[0] + M0[1]
            M_1_analytical = 0
            H_2_analytical = (M0[0] + M0[1])/rho_water/A0[1]
            H_1_analytical = 0

        result_analytical['t'].append(time_vector[i])
        result_analytical['M_1'].append(M_1_analytical)
        result_analytical['M_2'].append(M_2_analytical)
        result_analytical['H_1'].append(H_1_analytical)
        result_analytical['H_2'].append(H_2_analytical)
    return result_analytical
```

We can run the equation to get the analytical soluton for the input data defined above, and we can create a table (using pandas data frame) containing the analytical solution at each evaluated time. The code is shown below.

```python
result_analytical = analytical_solution(g=g, A0=A0, M0=M0, rho_water = rho_water, N_t=N_t, time_range=time_range, Cv_0=Cv_0)

data = {'Time, s': result_analytical['t'], 'H1, m':result_analytical['H_1'], 'H2, m':result_analytical['H_2'],
        'M1, kg':result_analytical['M_1'],'M2, kg':result_analytical['M_2']}
pd.DataFrame(data)
```

      Time, s
      H1, m
      H2, m
      M1, kg
      M2, kg

      0
      0.0
      0.500000
      0.000000
      25.000000
      0.000000

      1
      1.0
      0.369739
      0.065131
      18.486929
      6.513071

      2
      2.0
      0.259097
      0.120451
      12.954859
      12.045141

      3
      3.0
      0.168076
      0.165962
      8.403788
      16.596212

      4
      4.0
      0.096674
      0.201663
      4.833718
      20.166282

      5
      5.0
      0.044893
      0.227554
      2.244647
      22.755353

      6
      6.0
      0.012732
      0.243634
      0.636577
      24.363423

      7
      7.0
      0.000190
      0.249905
      0.009506
      24.990494

      8
      8.0
      0.000000
      0.250000
      0.000000
      25.000000

      9
      9.0
      0.000000
      0.250000
      0.000000
      25.000000

      10
      10.0
      0.000000
      0.250000
      0.000000
      25.000000

## Implementation using _numerous_

### Preliminary steps
The first step for the implementation is to include all the relevant _numerous_ modules.
For this tutorial we need:
* _Item_ for defining item objects
* _Model_ for defining model objects
* _ConnectorTwoWay_ for defining a special connectors object
* _Subsystems_ for defining the subsystem object
* _Simulation_ for defining the simulation
* _Equation_ and equation decorator for objects of equation class

Moreover, we will need HistoryDataFrame to store results.

```python
# We include all the relevant modules from numerous:
from numerous.engine.system import Item
from numerous.engine.model import Model
from numerous.engine.system import Subsystem
from numerous.engine.simulation import Simulation
from numerous.engine.system import ConnectorTwoWay
from numerous.engine.variables import VariableType, VariableDescription, OverloadAction

from numerous.multiphysics import Equation
from numerous.multiphysics import EquationBase

```

### Define Tank Equation
The first item that we will model is the tank. Before creating the tank item, we need to define the equation to apply, and we thus create a Tank_Equation item using Equation class. We need to define all the parameters and constants of the equation, by using the method ` Equation.add_parameter ` .  _g_ (gravitational accelleration) is the only variable defined as a constant, as its value cannot be modified.

Note that the line of code
``` python
super().__init__(tag='for info only')
```
in `__init__` is a necessary line to create any class using _numerous_ in the current release.

The only _state_ variable in the tank equation is given by the tank height, which is determined by the differential equation presented in the description above and reported in the equation definition.

```python
class Tank_Equation(EquationBase):
    def __init__(self, tag="tank_equation", H=0.5, rho=1000, A0=0.05, g=9.81):
        super().__init__(tag='tank_equation')

        self.add_state('H', H)          # [m] Liquid height in the tank
        self.add_parameter('rho', rho)  # [kg/m3] Liquid density
        self.add_parameter('A0', A0)    # [m2] Tank cross-sectional area
        self.add_constant('g', g)       # [m/s2] Gravitational acceleration
        self.add_parameter('mdot', 0)   # [kg/s] Mass flow rate

    @Equation()
    def eval(self, scope):
        # Differential equation for mass conservation in a general tank with a mass flow rate entering or leaving the tan
        scope.H_dot = scope.mdot / scope.rho / scope.A0
```

### Define Valve Equation
The second item that we will model is the valve. Before creating the valve item, we need to define the equation to apply, and we thus create a `Valve_Equation` item using `EquationBase` class.

Please, note the use of the **global variable** time, which is defined as ` scope.globals.time ` in _numerous_ , and thus it has not to be specified as the other parameters and constants.

Note that in the `Valve_Equation` no state is defined, since the valve characteristic parameter is a function of time, but we have an explicit formulation for it.

```python
# We define the equation (using EquationBase class) determining the mass flow rate across the valve
class Valve_Equation(EquationBase):
    def __init__(self, Cv_0=0.1, rho=1000, g=9.81, omega=1):
        super().__init__(tag='valve_equation')

        self.add_parameter('omega', omega)   # [rad/sec]   Angular frequency of valve characteristic parameter
        self.add_parameter('Cv_0', Cv_0)     # [(kg/s)/Pa] Amplitude of valve characteristic parameter
        self.add_parameter('mdot1', 0)       # [kg/s]      Mass flow rate in one side of the valve
        self.add_parameter('mdot2', 0)       # [kg/s]      Mass flow rate in the other side of the valve
        self.add_parameter('H1', 0)          # [m]         Liquid height in the tank 1 connected to the valve (top tank)
        self.add_parameter('H2', 0)          # [m]         Liquid height in the tank 2 connected to the valve (bottom tank)
        self.add_parameter('rho', rho)       # [kg/m3]     Liquid density
        self.add_constant('g', g)            # [m/s2]      Gravitational acceleration
        self.add_parameter('Cv', Cv_0)       # [(kg/s)/Pa] Valve characteristic parameter

    @Equation()
    def eval(self,scope,global_variables):
        scope.Cv      = scope.Cv_0 * (np.sin(scope.omega * global_variables.time)+1)           #[(kg/s)/Pa]
        deltaP        = scope.rho * scope.g * (scope.H1)                                  #[Pa]
        mdot          = np.sign(deltaP) * np.sqrt(np.absolute(deltaP)) * scope.Cv         #[kg/s]
        # The valve will be associated with two mass flow rates (one leaving and one entering the component),
        #which - for conservation of mass - have the same magnitude and opposite sign
        scope.mdot1   = -mdot                                                             #[kg/s]
        scope.mdot2   = mdot                                                              #[kg/s]

```

### Define Tank as Item
We define the `Tank` class as an `Item` class. We then create a namespace 'v1' to contain the variables for the `Tank_Equation`.
The equation is associated to the namespace using the `add_equations` method, as shown in the code below.

```python
class Tank(Item):
    def __init__(self, tag="tank", H=0.5, rho=1000, A0=0.05, g=9.81):
        super(Tank, self).__init__(tag)

        v1 = self.create_namespace('v1')
        v1.add_equations([Tank_Equation(H=H, rho=rho, A0=A0, g=g)])
```

### Define Valve as ConnectorTwoWay
Once that we have defined the equation describing the mass flow rate flowing through the valve, we need to create the Valve as a class `ConnectorTwoWay` and to assign an equation to it.
`ConnectorTwoWay` is a special case of a `Connector` class, and the reader is referred to _numerous_ documentation for an exhuastive explanation. The peculiarity of this connector is the possibility of defining two sides, i.e. variables can be binded to the connectors by specifying to different items as sides.

In the code lines
``` python
super().__init__(tag, side1_name='side1', side2_name='side2')
```
we have to specify the names of the two sides.

The steps that we have to take are the following (refer to the numbering in the code comments # to see which lines of codes belong to the different steps)
1. We create a namespace 'v1' to contain the variables for the valve equation. This is done using the Item method `Item.create_namespace `. The namespace is then associated to an equation using the `add_equations` method.
2. We create variables at each side of the connector item, and we associated them to the same namespace containing the valve equation. The variables must be created because when we first instantiate the ConnectorTwoWay object no information on side1 and side2 is passed.
3. The binding between the ConnectorTwoWay and the two items at each side is done, using the variables previosuly created in the name space. In this particular example:
    * the value of v1.H1 and v1.H2 (liquid heights of the tanks connected to the valve, stored inside the valve object) must point to the respective tank heights in the two side objects. This implies that the value of H is determined by the tank equation and not by the valve equation.
    * the value of the mass flow rate entering or leaving each tank (for example the value self.side1.v1.mdot stored inside the side1 object (tank 1)) must point to the mass flow rate flowing through the valve (in this case determined by the valve equation)

```python
# Define the valve as a connector item - connecting two tanks
class Valve(ConnectorTwoWay):
    def __init__(self, tag="valve", Cv_0=0.1, rho=1000, g=9.81, omega=0):
        super().__init__(tag, side1_name='side1', side2_name='side2')

        #1 Create a namespace for mass flow rate equation and add the valve equation
        v1 = self.create_namespace('v1')
        v1.add_equations([Valve_Equation(Cv_0=Cv_0, rho=rho, g=g, omega=omega)])

        #2 Create variables H and mdot in side 1 adn 2
           #(side 1 represents a connection with one tank, with related liquid height H_1)
           #(side 1 represents a connection with the second tank, with related liquid height H_2)
        self.side1.v1.create_variable(name='H')
        self.side1.v1.create_variable(name='mdot')
        self.side2.v1.create_variable(name='H')
        self.side2.v1.create_variable(name='mdot')

        # Map variables between binding and internal variables for side 1 and 2
        # This is needed to update the values of the variables in the binding according to the equtions of the items
        v1.H1              = self.side1.v1.H
        v1.H2              = self.side2.v1.H

        self.side1.v1.mdot = v1.mdot1
        self.side2.v1.mdot = v1.mdot2

```

###  Create the sub-system of components
After defining all the classes for the items that will consitute the system, we are ready for the system assembly.
We create a special class of `Subsystem`, inside which we: (refer to the numbering in the code comments # to see which lines of codes belong to the different steps)
1. create the gravitational accelleration constant and assign a value to it
2. create two instances of the class Tank called Tank_1 (top tank) and Tank_2 (bottom tank)
3. create one instance of the class Valve called Valve_1
4. bind Tank_1 and Tank_2 by assigning each of them to the two sides of Valve_1. we use the `ConnectorTwoWay.bind` method for this.
5. register the instanciated items in the Two_Tanks class

The inputs needed to the subsystem are:
* H0, which is a vector containing the initial state of the system (initial liquid height of Tank_1 and Tank_2)
* Cv_0 and omega, which are amplitude and angular frequency of the valve characteristic parameter
We assume that geometry (A0) and liquid (rho) are given by the input data as fixed values

```python
# Define the subsystem composed by two tanks and one valve connecting them
class Two_Tanks(Subsystem):
    def __init__(self, tag, H0, Cv_0, omega):
        super().__init__(tag)

        #1. Gravitational acceleration
        g = 9.81

        #2. Instances of Tank class
        Tank_1 = Tank('tank_1', H=H0[0], rho=rho_water, A0=A0[0], g=g)
        Tank_2 = Tank('tank_2', H=H0[1], rho=rho_water, A0=A0[1], g=g)

        #3. Valve_1 is one instance of valve class
        Valve_1 = Valve('valve_1', Cv_0=Cv_0, rho=rho_water, g=g, omega=omega)

        #4. Binding
        Valve_1.bind(side1=Tank_1, side2=Tank_2)

        #5. Register all the instanciated items in the sub-system
        self.register_items([Tank_1, Tank_2, Valve_1])
```

###  Create the system model and simulation
Finally we are ready to define the model and the simulation of the implemented system, and we do it by creating a function named `t_1_item_model`. The inputs to the function are given by:
* H0, Cv_0, omega, which represent initial conditions of the tanks, and valve characteristic
* hdf, which is the historian where the simulation results will be stored
* t_start_sim and t_stop_sim determing the time span in which the system will be simulated

The steps are the following:
1. First a model object m1 is instantiated based on the `Two_Tanks` subsystem with given inputs
2. A simulation s1 is connected to the model object, and some solver settings are chosen. The parameter 'num' is used to specify the number of steps at which the solution is evaluated between t_start_sim and t_stop_sim
3. The `simulation.solve` method is called, and the solution is returned as output of the function

```python
def t_1_item_model(H0, Cv_0, omega, t_start_sim, t_stop_sim):

    # 1. Instantiate the model
    m1 = Model(Two_Tanks('subsystem', H0, Cv_0, omega))
    # 2. Setting up the simulation:
    s1  = Simulation(m1, t_start=t_start_sim, t_stop=t_stop_sim, num=5000)
    # 3. Call the solve method
    sol = s1.solve()
    return s1, sol
```

## Running the simulations - Case 1: Constant Cv
We impose a Cv to be constant in time by setting the angular frequency $\omega$ to be equal to 0 (i.e. the cosine of 0 will be equal to 1 at each time step).
To obtain the solution of the system for Case 1, we simply need to call the `t_1_item_model` function with the inputs defined by the input data, and omega = 0.
The progress bar is shown below.

```python
omega = 0

# Collect the historical data at each time step
s1, sol = t_1_item_model(H0, Cv_0,omega, t_start_sim, t_stop_sim)
```

### Plotting: comparison vs. analytical solution
To plot the solution, we add the following code which uses `plotly`. We shall not go into detail on the code, but simply include it here as it's used for the purpose illustrating the results of this tutorial only.

We can plot the solution by accessing the `Model` object `historian`, which contains a log of all variables as a `Pandas` dataframe. The `Simulation` object, `sim1` contains the `model` object and the time logged variables are accessible through `sim1.model.historian`.

```python

```

```python
# Plot the comparison
# Nb: the package plotly is needed for the plotting
from plotly import __version__
from plotly.offline import download_plotlyjs, init_notebook_mode, plot, iplot
import plotly.graph_objs as go

fig = go.Figure()
fig.add_trace(go.Scatter(
            x=hdf.df.index.total_seconds(), y=hdf.df['subsystem.tank_1.v1.H'],
            name='Tank 1 - numerical', mode='lines',line_color='rgba(102,51,255,1)'))
fig.add_trace(go.Scatter(
            x=result_analytical['t'], y=result_analytical['H_1'],
            name='Tank 1 - analytical', mode='markers', marker_color='rgba(102,51,255,1)'))
fig.add_trace(go.Scatter(
            x=hdf.df.index.total_seconds(), y=hdf.df['subsystem.tank_2.v1.H'],
            name='Tank 2 - numerical', mode='lines',line_color='rgba(152, 0, 0, .8)'))
fig.add_trace(go.Scatter(
            x=result_analytical['t'], y=result_analytical['H_2'],
            name='Tank 2 - analytical',  mode='markers',marker_color='rgba(152, 0, 0, .8)'))

fig.update_layout(title='Liquid height in tanks - analytical vs. numerical solution',
                  yaxis_zeroline=False, xaxis_zeroline=False,xaxis_title="time, sec",
                 yaxis_title="liquid height H, m",)
fig.show()
```

```python
fig = go.Figure()
fig.add_trace(go.Scatter(
            x=hdf.df.index.total_seconds(), y= hdf.df['subsystem.tank_1.v1.H']*rho_water*A0[0],
            name='Tank 1 - numerical', mode='lines',line_color='rgba(102,51,255,1)'))
fig.add_trace(go.Scatter(
            x=result_analytical['t'], y=result_analytical['M_1'],
            name='Tank 1 - analytical', mode='markers', marker_color='rgba(102,51,255,1)'))
fig.add_trace(go.Scatter(
            x=hdf.df.index.total_seconds(), y=hdf.df['subsystem.tank_2.v1.H']*rho_water*A0[1],
            name='Tank 2 - numerical', mode='lines',line_color='rgba(152, 0, 0, .8)'))
fig.add_trace(go.Scatter(
            x=result_analytical['t'], y=result_analytical['M_2'],
            name='Tank 2 - analytical', mode='markers',marker_color='rgba(152, 0, 0, .8)'))

fig.update_layout(title='Liquid mass in tanks - analytical vs. numerical solution',
                  yaxis_zeroline=False, xaxis_zeroline=False,xaxis_title="time, sec",
                 yaxis_title="liquid mass M, kg",)
fig.show()

```

## Running the simulations - Case 2: Time dependent Cv
We assign now a value to the parameter $\omega$, so that the valve has a characteristic parameter that is dependent from time, with a sinusoidal behaviour.

To obtain the solution of the system for Case 2 the procedure is analogous to the previous case.

```python
omega_2 = 1.5 # rad/s

# Solve the model and collect hystorical data
hdf_2     = SimpleHistoryDataFrame()
s2, sol_2 = t_1_item_model(H0, Cv_0,omega_2, hdf_2, t_start_sim, t_stop_sim)
```

### Plotting the results

```python
# Plot the results
# Nb: the package plotly is needed for the plotting
from plotly import __version__
from plotly.offline import download_plotlyjs, init_notebook_mode, plot, iplot
import plotly.graph_objs as go

fig = go.Figure()
fig.add_trace(go.Scatter(
            x=hdf_2.df.index.total_seconds(), y=hdf_2.df['subsystem.tank_1.v1.H'],
            name='Tank 1', mode='lines',line_color='rgba(102,51,255,1)'))
fig.add_trace(go.Scatter(
            x=hdf_2.df.index.total_seconds(), y=hdf_2.df['subsystem.tank_2.v1.H'],
            name='Tank 2', mode='lines',line_color='rgba(152, 0, 0, .8)'))

fig.update_layout(title='Liquid height in tanks',
                  yaxis_zeroline=False, xaxis_zeroline=False,xaxis_title="time, sec",
                 yaxis_title="liquid height H, m",)
fig.show()
```

```python
fig = go.Figure()
fig.add_trace(go.Scatter(
            x=hdf_2.df.index.total_seconds(), y= hdf_2.df['subsystem.tank_1.v1.H']*rho_water*A0[0],
            name='Tank 1', mode='lines',line_color='rgba(102,51,255,1)'))
fig.add_trace(go.Scatter(
            x=hdf_2.df.index.total_seconds(), y=hdf_2.df['subsystem.tank_2.v1.H']*rho_water*A0[1],
            name='Tank 2', mode='lines',line_color='rgba(152, 0, 0, .8)'))

fig.update_layout(title='Liquid mass in tanks',
                  yaxis_zeroline=False, xaxis_zeroline=False,xaxis_title="time, sec",
                 yaxis_title="liquid mass M, kg",)
fig.show()
```

```python
fig = go.Figure()
fig.add_trace(go.Scatter(
            x=hdf_2.df.index.total_seconds(), y= hdf_2.df['subsystem.valve_1.v1.Cv'],
            name='Cv', mode='lines',line_color='rgba(41, 241, 195, 1)'))
fig.update_layout(title='Valve characteristc parameter',
                  yaxis_zeroline=False, xaxis_zeroline=False,xaxis_title="time, sec",
                 yaxis_title="Cv, (kg/s)/Pa",)
fig.show()
```


```python
import numpy as np
```

# Utilities

This notebook contains utility code that is useful to multiple notebooks.

## Derivative Filter

In many control schemes that follow trajectories, higher order derivatives are often needed. This section discusses an implementation of a digital derivative based in Laplace system theory.

Recall from system theory that the Laplace transform is a useful way of analyzing linear time-invariant (LTI) systems. It takes time-domain systems and expresses them in the Laplace domain (a superset of the Fourier frequency domain). The unilateral Laplace transform is defined by
$$
\begin{equation}
F(s) = \int_0^\infty f(t) e^{-st} dt,
\end{equation}
$$
where $t$ is a time variable and $s$ is the Laplace variable. This transform can be thought as projecting a time-domain function onto exponentials of varying phase.

This transform is particularly useful when analyzing LTI systems of differential equations. In particular, the Laplace transform of the time-derivative of a continuous system $f(t)$ can be found to be
$$
\begin{equation}
\frac{d}{dt} f(t) \stackrel{\mathcal{L}}{\rightleftharpoons} sF(s).
\end{equation}
$$

Therefore, differentiation in the time-domain corresponds to multiplication by $s$ in the Laplace domain. Although a pure derivative in the Laplace domain is causal, it is not realizable (i.e., can you build an RLC circuit that realizes $F(s)=s$?). Therefore, a band-limited derivative (i.e., a *dirty derivative*) is used:
$$
\begin{equation}\label{eq:dirty-derivative}
G(s) = \frac{s}{\tau s + 1}.
\end{equation}
$$

From analog filter design, remember that a first-order low-pass filter with cutoff frequency $\omega_c$ is defined in the Laplace domain as
$$
\begin{equation}\label{eq:lpf}
H_{LPF}(s) = \frac{\omega_c}{s + \omega_c} = \frac{1}{s/\omega_c + 1},
\end{equation}
$$
and a first-order high-pass filter is
$$
\begin{equation}\label{eq:hpf}
H_{HPF}(s) = \frac{s}{s + \omega_c} = \frac{s/\omega_c}{s/\omega_c + 1}.
\end{equation}
$$

Note that the cutoff frequency is related to the filter time-constant (and RC circuits) by
$$
\begin{equation}
\tau = RC = \frac{1}{2\pi f_c} = \frac{1}{\omega_c}.
\end{equation}
$$

From this discussion, we can see that the dirty derivative \eqref{eq:dirty-derivative} can be thought of as a filtered version of a pure derivative with bandwidth $1/\tau$. This is useful as the LPF will prevent the derivative operator from picking up high-frequency components of the input signal and amplifying noise.

A common value of $\tau$ is $0.5$ ($20$ Hz). A higher $\tau$ corresponds to less bandwidth and more rejection of high-frequency components, which results in a smoother output.

### Digital Implementation

In order to implement a derivative filter in a computer, it of course must be discretized. The technique used here is to map $G(s)$ to the $z$-domain via the bilinear transform (AKA, Tustin approximation)
$$
\begin{equation}
s \mapsto \frac{2}{T}\frac{1-z^{-1}}{1+z^{-1}} ,
\end{equation}
$$
where $T$ is the sample period. Once we have an expression in the $z$-domain, the inverse $z$-transform can be used to give a discrete-time implementation of the dirty derivative.

#### Resources

- [SE.DSP: First-Derivative Analog Filter](https://dsp.stackexchange.com/questions/41109/first-derivative-analog-filter)
- [Blog: Causal, but not Realizable](http://blog.jafma.net/2015/10/04/differentiation-derivative-is-causal-but-not-exactly-realizable/)

```python
class DirtyDerivative:
    """Dirty Derivative

    Provides a first-order derivative of a signal.

    This class creates a filtered derivative based on a
    band-limited low-pass filter with transfer function:

        G(s) = s/(tau*s + 1)

    This is done because a pure differentiator (D(s) = s)
    is not realizable.
    """
    def __init__(self, order=1, tau=0.05):
        # time constant of dirty-derivative filter.
        # Higher leads to increased smoothing.
        self.tau = tau

        # Although this class only provides a first-order
        # derivative, we use this parameter to know how
        # many measurements to ignore so that the incoming
        # data is smooth and stable. Otherwise, the filter
        # would be hit with a step function, causing
        # downstream dirty derivatives to be hit with very
        # large step functions.
        self.order = order

        # internal memory for lagged signal value
        self.x_d1 = None

        # Current value of derivative
        self.dxdt = None

    def update(self, x, Ts):
        # Make sure to store the first `order` measurements,
        # but don't use them until we have seen enough
        # measurements to produce a stable output
        if self.order > 0:
            self.order -= 1
            self.x_d1 = x
            return np.zeros(x.shape)

        # Calculate digital derivative constants
        a1 = (2*self.tau - Ts)/(2*self.tau + Ts)
        a2 = 2/(2*self.tau + Ts)

        if self.dxdt is None:
            self.dxdt = np.zeros(x.shape)

        # calculate derivative
        self.dxdt = a1*self.dxdt + a2*(x - self.x_d1)

        # store value for next time
        self.x_d1 = x

        return self.dxdt
```

---

## Rotation Matrices

Throughout this notebook, we use *right-handed*, *passive* rotations to express the same geometrical vector in various coordinate frames. When Euler angles are used, the *intrinsic* 3-2-1 (Z-Y-X) sequence is used. This means that a yaw ($\psi$) rotation is first performed in $\mathcal{F}_A$ around the $\mathbf{k}^A$ axis to get to $\mathcal{F}_B$. Then, a pitch ($\theta$) rotation is performed about the $\mathbf{j}^B$ axis of frame B to get to $\mathcal{F}_C$. Lastly, a roll ($\phi$) rotation is performed about the $\mathbf{i}^C$ axis to get to $\mathcal{F}_D$. The order of rotation is important, and if data from $\mathcal{F}_A$ is meant to be expressed in $\mathcal{F}_D$, the rotations are composed as
$$
\begin{equation}
x^D = R_C^D(\phi) R_B^C(\theta) R_A^B(\psi)x^A = R_A^D(\phi,\theta,\psi)x^A = R_A^D x^A.
\end{equation}
$$

Python implementations of these passive rotations are defined below.

```python
# input angle in radians
def rotx(ph): return np.array([[1,0,0],[0,np.cos(ph),np.sin(ph)],[0,-np.sin(ph),np.cos(ph)]])
def roty(th): return np.array([[np.cos(th),0,-np.sin(th)],[0,1,0],[np.sin(th),0,np.cos(th)]])
def rotz(ps): return np.array([[np.cos(ps),np.sin(ps),0],[-np.sin(ps),np.cos(ps),0],[0,0,1]])
def rot3(ph,th,ps): return rotx(ph).dot(roty(th).dot(rotz(ps)))
# input angle in degrees
def rotxd(ph): return rotx(np.radians(ph))
def rotyd(th): return roty(np.radians(th))
def rotzd(ps): return rotz(np.radians(ps))
def rot3d(ph,th,ps): return rot3(np.radians(ph),np.radians(th),np.radians(ps))
```

---

## PID Controller

One of the simplest forms of feedback control is the proportional-integral-derivative (PID) controller. It is perhaps the most widely used form of control because it is an intuitive technique for minimizing error. However, stability can only be guaranteed for second-order systems.

### Digital Implementation

In order to implement a PID controller on a computer, we need to discretize our continuous expression for the integral and derivative terms of the PID controller.

```python
class SimplePID:
    def __init__(self, kp, ki, kd, min, max, tau=0.05):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.min = min
        self.max = max
        self.tau = tau

        self.derivative = 0.0
        self.integral = 0.0

        self.last_error = 0.0

    @staticmethod
    def _clamp(v, limit):
        return v if np.abs(v) < limit else limit*np.sign(v)

    def run(self, error, dt, derivative=None, pclamp=None):

        # P term
        if self.kp:
            # Proportional error clamp, it specified
            e = error if pclamp is None else self._clamp(error, pclamp)
            p_term = self.kp * e
        else:
            p_term = 0.0

        # D term
        if self.kd:
            if derivative:
                self.derivative = derivative
            elif dt > 0.0001:
                self.derivative = (2.0*self.tau - dt)/(2.0*self.tau + dt)*self.derivative + 2.0/(2.0*self.tau + dt)*(error - self.last_error)
            else:
                self.derivative = 0.0
            d_term = self.kd * self.derivative
        else:
            d_term = 0.0

        # I term
        if self.ki:
            self.integral += (dt/2.0) * (error + self.last_error)
            i_term = self.ki * self.integral
        else:
            i_term = 0.0

        # combine
        u = p_term + d_term + i_term

        # saturate
        if u < self.min:
            u_sat = self.min
        elif u > self.max:
            u_sat = self.max
        else:
            u_sat = u

        # integrator anti-windup
        if self.ki:
            if abs(p_term + d_term) > abs(u_sat):
                # PD is already saturating, so set integrator to 0 but don't let it run backwards
                self.integral = 0
            else:
                # otherwise only let integral term at most take us just up to saturation
                self.integral = (u_sat - p_term - d_term) / self.ki

        # bookkeeping
        self.last_error = error

        return u_sat
```

---

## Numerical Integration

Before our discussion of the dynamical equations of motion that describe the time evolution of the quadrotor in $\text{SE}(3)$, we should discuss how to properly integrate these differential equations numerically for use in a simulation. The two most common types of numerical integrators are Euler's method and the fourth order Runge-Kutta method, or RK4.

### The Euler Method

The Euler method is an explicit algorithm that uses the limit definition of the derivative:
$$
\frac{df}{dt} = \lim_{h\to0} \frac{f(t + h) - f(t)}{h}.
$$
Let $g(t) \triangleq \frac{df}{dt}(t)$. Disregarding the limit and rearranging the difference quotient yields
$$
f(t+h) = f(t) + g(t)h.
$$
Using the step size $h$ as the sample period and using discrete notation, we have
$$
f[k+1] = f[k] + g[k] T_s.
$$

### RK4

Of course, the Euler Method is a first-order, rough approximation of differential equations. Given a differential equation $\dot{y} = f(t,y)$, the RK4 method is given by
$$
\begin{align}
y_{n+1}	&=	y_{n}+\frac{h}{6}\left(k_{1}+2k_{2}+2k_{3}+k4\right) \\
t_{n+1}	&=	t_{n}+h,
 \end{align}
$$

where
$$
\begin{align}
k_{1} &= f\left(t_{n},y_{n}\right) \\
k_{2} &= f\left(t_{n}+\frac{h}{2},y_{n}+\frac{h}{2}k_{1}\right) \\
k_{3} &= f\left(t_{n}+\frac{h}{2},y_{n}+\frac{h}{2}k_{2}\right) \\
k_{4} &= f\left(t_{n}+h,y_{n}+hk_{3}\right).
\end{align}
$$

For an expository derivation of RK4, see [2].

```python
def rk4(f, y, dt):
    """Runge-Kutta 4th Order

    Solves an autonomous (time-invariant) differential equation of the form dy/dt = f(y).
    """
    k1 = f(y)
    k2 = f(y + dt/2*k1)
    k3 = f(y + dt/2*k2)
    k4 = f(y + dt  *k3)
    return y + (dt/6)*(k1 + 2*k2 + 2*k3 + k4)
```

#### RK4 Example

Consider the autonomous evolution of position $x \in \mathbb{R}$ as
$$
\dot{x} = \cos(\omega(t))
$$

where
$$
\omega(t) =
\begin{cases}
    2 \pi t, & 0 < t < 2.5 \\
    4 \pi t, & 2.5 \leq t < 5 \\
\end{cases}.
$$

Note that this equation is not smooth (there is a jump at $t = 2.5$), but we are still able to numerically integrate it.

```python
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt

# simulation timing
Tf = 5
Ts = 0.01
N = int(Tf/Ts)

# initital condition
x = 0

# simulation history
x_hist = np.zeros((N,1))

for i in range(N-1):
    # dynamics
    freq = 1 if i < N//2 else 2
    v = np.cos(2*np.pi*freq*(Ts*i))
    f = lambda x: v

    # propagate dynamics by solving the differential equation
    x = rk4(f, x, Ts)

    # add to history
    x_hist[i+1] = x

plt.plot(np.arange(0, Tf, Ts), x_hist)
plt.grid(); plt.xlabel('Time (s)'); plt.ylabel('Position (m)')
plt.show()
```

### Lie Group Integration

Note that the above formulations we assume Euclidean spaces instead of matrix Lie groups.

---


(c) Juan Gomez 2019. Thanks to Universidad EAFIT for support. This material is part of the course Introduction to Finite Element Analysis

# Interpolation in Two-Dimensional Domains

## Introduction

Here we will extend the one-dimensional interpolation scheme studied previously to the more general case of a two-dimensional domain. From the geometric point of view we will also see that a **finite element** is just a canonical spatial domain described by nodal points and the corresponding set of interpolation (or **shape**) functions. Thus this will be the first formal definition of a finite element in the course. **After completing this notebook you should be able to:**

* Recognize the problem of interpolation in two-dimensional domains as the application of the known one-dimensional scheme.

* Formalize the concept of a finite element as a canonical interpolation space with prescribed interpolation functions.

* Propose interpolation schemes for general two-dimensional domains.

## Two-dimensional domain
Consider the two-dimensional square domain shown in the figure and where we want to approximate, via interpolation, a scalar (or vector) valued fuction $f=f(x,y)$. For that purpose the black-dots in the figure, represent nodal or sampling points where the function is assumed to be known. As discussed in the **Class Notes (Section 1.4)** the interpolating polynomial, in this case denoted by $p(x,y)$ is built like:

$$p(x,y) = H^Q(x,y)f^Q$$

where $Q = 1,...,N$ for a *N*-noded domain and where $H^Q(x,y)$ are the interpolation polynomials which in the context of the finite element method are called **Shape Funtions**.

To built the bi-dimensional interpolation functions $ H^Q(x,y)$ we actually perform an iterated one-dimensional interpolation as described next.

Let $x^A$ and $x^B$ denote the x-coordinates of points A and B for the square domain, shown in the figure below and assume we would like to find the value of the function at point A.

Point A has an arbitrary y-coordinate but a constant $x = x^A$ x-coordinate, thus for an arbitrary point A along the 1-4 direction (see figure below) the interpolation scheme is still one-dimensional with only y-dependence as indicated by the label $f(y , x= A)$ in the figure. Using known one-dimensional Lagrange polynomials this y-dependence can be captured by:

$$f(x^A , y) = L^1(y)f^1 + L^4(y)f^4$$

Proceeding similarly for an arbitrary point B along the 2-3 direction leads to:

$$f(x^B , y) = L^2(y)f^2 + L^3(y)f^3.$$

With $f^A$ and $f^B$ known the x-dependence can now be captured like:

$$f(x,y) = L^A(x) f(x^A,y) + L^B(x)f(x^B,y).$$

To arrive at the final 2D-shape functions we compute the polynomials $L^2(y)$, $L^3(y)$, $ L^A(x)$ and $ L^B(x)$ and replace them in the expressions above. In the case of an element of side 2.0 these functions take the form:

\begin{align*}
H^1(x,y) & = L^1(x)L^1(y) \equiv \frac14(1-x)(1-y)\\
H^2(x,y) & = L^2(x)L^1(y) \equiv \frac14(1+x)(1-y)\\
H^3(x,y) & = L^2(x)L^2(y) \equiv \frac14(1+x)(1+y)\\
H^4(x,y) & = L^1(x)L^2(y) \equiv \frac14(1-x)(1+y).
\end{align*}

Since along each line 1-4 or 2-3 one-dimensional interpolation is being used, the formulated element is termed a bi-linear element.

### A Canonical Finite Element

In the following subroutine we code the final form $H^Q(x,y)$ of the shape functions instead of directly computing the fundamental 1D-polynomials of the form $L^I(y)$ and computing the iterated interpolation. The subroutine, named here **sha4()** stores the functions in a matrix structure which depends on $x$ and $y$. Here we assume that the element is a perfect square of side $\mathcal l=2.0$ with nodal points at the corners thus allowing for linear interpolation along each face.

```python
%matplotlib notebook
import matplotlib.pyplot as plt
import numpy as np
import sympy as sym
from scipy import interpolate
```

```python
def sha4(x,y):
    """
    Compute the shape functions for bi-linear
    square element of size 2.0.
    """
    sh=sym.zeros(4)
    sh[0] =(1.0/4.0)*(1 - x)*(1 - y)
    sh[1] =(1.0/4.0)*(1 + x)*(1 - y)
    sh[2] =(1.0/4.0)*(1 + x)*(1 + y)
    sh[3] =(1.0/4.0)*(1 - x)*(1 + y)
#
    return sh
```

This square-element is a **canonical** or reference element where it is easy to conduct the interpolation operation. In an actual finite element discretization the resulting quadrilateral elements are expected to be distorted with respect to this canonical element. In these cases interpolation is still conducted in the space of the canonical element and geometry and functions are nicely transformed using simple mathematics. However, those details will be discussed later.

The shape functions stored in the subroutine correspond to:

$$H = \frac14\begin{bmatrix}(1-x)(1-y)&(1+x)(1-y)&(1+x)(1+y)&(1-x)(1+y)\end{bmatrix}$$

**Questions:**

**(i) Write the element shape functions if the sub-domain is now the same squared element but now it also includes, in addition to the corner nodes, midside nodes to complete a total of 8 nodal points.**

**(ii) Make a copy of the subroutine **sha4()** and modify it to compute the shape functions for an 8-noded element.**

```python
x , y= sym.symbols('x y')
H = sha4(x , y)
```

## Interpolation over a square element.

In this step we consider a square element conformed by 4-nodal points located at the corners and where nodal values of a function are assumed to be known. We use these values together with the shape functions to derive an interpolating polynomial. The resulting polynomial is used next to generate approximated values of the function along a set of points conforming a grid used to visualize the solution. The grid of observation points is generated using the function **mgrid** from **numpy**.

Notice that the system of reference is placed at the center of the element thus $x\in\lbrack-1\;,\;1\rbrack$ and $y\in\lbrack-1\;,\;1\rbrack$. The 1D array **USOL[]** will store the value interpolated at each point of the grid.

To conduct the interpolation we will assume known nodal values of the function at a given point $(x , y)$ so we can obtain the interpolated value like:

$$u(x,y)\;=\;\left[H(x,y)\right]\left\{u\right\}$$

**(Add comments to clarify the relevant steps in the code below)**.

```python
li=-1.0
ls= 1.1
dl= 0.1
npts=int((ls-li)/dl)
USOL = np.zeros((npts, npts, 1))
xx, yy = np.mgrid[li:ls:npts*1j, li:ls:npts*1j]
#Try different values of the function at the nodal points
u = sym.Matrix(4, 1, [-0.2 ,0.2 ,-0.2 , 0.2])
#
for i in range(npts):
    for j in range(npts):
        NS =H.subs([(x, xx[i,j]), (y, yy[i,j])])
        up  = NS*u
        USOL[i, j, 0] = up[0]
plt.figure(1)
plt.contourf(xx , yy , USOL[:,:,0], cmap="RdYlBu")
```

    <IPython.core.display.Javascript object>

    <matplotlib.contour.QuadContourSet at 0xb1ef1f320>

### Glossary of terms

**Canonical finite element:** Undistorted constant size sub-domain with fixed shape functions. In a practical case the elements differ in size and level of distortion, however all of them are geometrically transformed to the canonical element.

**Shape functions:** Interpolation functions formulated over a canonical element.

**Mesh:** A set of finite elements covering a given computational domain. A mesh is refined when the characteristic element size reduces generating a larger number of elements to cover the same compuattional domain.

### Class activity
**Problem 1**

Extend the 2D interpolation scheme discussed above to the case of a vector valued function in the context of linear elasticity. For that purpose:

* Assume that at each nodal point of the square domain the displacement vector with horizontal and vertical componentes denoted by $u$ and $v$ respectively is known.
* Using these nodal values compute the horizontal and vertical displacement components over the element.
* Using these same nodal values compute the strain field given by:

$$\varepsilon_{xx}=\frac12\left(\frac{\partial u}{\partial x}\right)$$

$$\varepsilon_{yy}=\frac12\left(\frac{\partial v}{\partial y}\right)$$

$$\gamma_{yy}=\left(\frac{\partial u}{\partial y}+\frac{\partial v}{\partial x}\right)$$

* Store the shape function derivatives in a separate matrix $B$.

**Problem 2**

In a realistic finite element problem and due to geometric irregularities in the computational domain all the elements of the mesh would have different geometric paramters. These differences not only complicate the interpolation process but even create serious problems on coding a systematic approach. In practice every real (distorted) element is transformed to a canonical element in such a way that the interpolation process is conducted in the canonical space. The figure below shows the relation between these two spaces emphasizing the fact that there is a one to one connecion between a point in both spaces. The mathematical details of the transformation are provided in the **Class Notes.**

* Compute the transformation between the distorted and canonical element spaces required to conduct two-dimensional interpolation in a realistic case.

```python
from IPython.core.display import HTML
def css_styling():
    styles = open('./nb_style.css', 'r').read()
    return HTML(styles)
css_styling()
```

<link href='http://fonts.googleapis.com/css?family=Fenix' rel='stylesheet' type='text/css'>
<link href='http://fonts.googleapis.com/css?family=Alegreya+Sans:100,300,400,500,700,800,900,100italic,300italic,400italic,500italic,700italic,800italic,900italic' rel='stylesheet' type='text/css'>
<link href='http://fonts.googleapis.com/css?family=Source+Code+Pro:300,400' rel='stylesheet' type='text/css'>

```python

```


```python
# environment setting, before any codes
import numpy as np
from scipy import integrate
%matplotlib inline
%config InlineBackend.figure_format = 'retina'
import matplotlib.pyplot as plt
from ipywidgets import interact, interactive, fixed, interact_manual
import ipywidgets as widgets
from IPython.display import clear_output, display
```

```python
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.colors import cnames
from matplotlib import animation
```

```python
def ExEuler(yfun, tn, yn, h):
    return yn + h*yfun(tn, yn)
```

```python
def yfun1(t, y):
    return (1-2*t)*y
```

\begin{align}
y'(t) & = (1 - 2t)y(t) \\
y(0) & = 1
\end{align}

```python
def solve_example1(N, T):
    fig = plt.figure()

    def ex_fun(t):
        return np.exp(t-t**2)

    num = 1000
    t = np.linspace(0,T, num+1)
    plt.plot(t, ex_fun(t), 'b-', lw=2)

    # Setting
    t = np.linspace(0, T, N+1)
    dt = np.diff(t)
    y = np.zeros(N+1)
    y[0] = 1

    # Explicit Euler Method
    for i in range(N):
        y[i+1] = ExEuler(yfun1, t[i], y[i], dt[i])

    # Implicit Euler Method
    #for i in range(N):
    #    y[i+1] = y[i]/(1-(1 - 2*t[i+1])*dt[i])

    # Trapezoid Method
    #for i in range(N):
    #    y[i+1] = y[i]*(1 + dt[i]/2*(1 - 2*t[i]))/(1-(1 - 2*t[i+1])*dt[i]/2)

    plt.plot(t, y, 'ro', markersize=8)

    plt.show()
```

```python
solve_example1(10,3)
```

```python
w = interactive(solve_example1, N=widgets.IntSlider(min=5,max=40,value=10), T=fixed(5))
display(w)
```

    interactive(children=(IntSlider(value=10, description='N', max=40, min=5), Output()), _dom_classes=('widget-in…

Now, we explore the Lorenz system of differential equations:

The Lorenz equations are a simplification of a miniature atmosphere model that he designed to study Rayleigh-Bénard convection, the movement of heat in a fluid, such as air, from a lower warm medium (such as the ground) to a higher cool medium (like the upper atmosphere). In this model of a two-dimensional atmosphere, a circulation of air develops that can be described by the following system of three equations:

\begin{align*}
 x' & = -s x + sy\\
 y' & = -xz + rx - y \\
 z' & = xy - bz
\end{align*}
The variable $x$ denotes the clockwise circulation velocity, $y$ measures the temperature difference between the ascending and descending columns of air, and $z$ measures the deviation from a strictly linear temperature profile in the vertical direction. The Prandtl number $s$, the Rayleigh number $r$, and $b$ are parameters of the system.

```python
def rk4(yfun, tn, yn, h):
    s1 = yfun(tn, yn)
    s2 = yfun(tn+h/2, yn+h/2*s1)
    s3 = yfun(tn+h/2, yn+h/2*s2)
    s4 = yfun(tn+h, yn + h*s3)
    return yn + h*(s1 + 2*s2 + 2*s3 + s4)/6
```

```python
def solve_lorenz_rk4(N, T, x0=0, y0=1, z0=1.05, s=10, r=28, b=8/3):

    fig = plt.figure()
    ax = fig.add_axes([0, 0, 1, 1], projection='3d')
    ax.axis('off')

    ax.set_xlim((-25, 25))
    ax.set_ylim((-35, 35))
    ax.set_zlim((0, 50))

    def yfun(t, y, s=10, r=28, b=8/3):
        z = np.zeros(3)
        z[0] = -s*y[0] + s*y[1]
        z[1] = -y[0]*y[2] + r*y[0] - y[1]
        z[2] = y[0]*y[1] - b*y[2]
        return z

    #y0 = -15 + 30*np.random.random(3)
    y0 = np.array([x0, y0, z0])

    t = np.linspace(0, T, N+1)
    dt = np.diff(t)

    y = np.zeros([N+1,3])
    y[0] = y0

    for i in range(N):
        y[i+1] = rk4(yfun, t[i], y[i], dt[i])

    s = 10
    c = np.linspace(0,1,N+1)
    for i in range(0,N+1-s,s):
        ax.plot(y[i:i+s+1,0], y[i:i+s+1,1], y[i:i+s+1,2], color=(1,c[i],0), alpha=0.4)
    plt.show()

```

```python
solve_lorenz_rk4(1000, 20)
```

```python
w = interactive(solve_lorenz_rk4, N=fixed(1000), T=fixed(20), x0=widgets.FloatSlider(min=-10,max=10), y0=widgets.FloatSlider(min=-10,max=10), z0=widgets.FloatSlider(min=-10,max=10))
display(w)
```

    interactive(children=(FloatSlider(value=0.0, description='x0', max=10.0, min=-10.0), FloatSlider(value=0.0, de…

```python
def solve_lorenz(N=10, angle=0.0, max_time=4.0, sigma=10.0, beta=8./3, rho=28.0):

    fig = plt.figure()
    ax = fig.add_axes([0, 0, 1, 1], projection='3d')
    ax.axis('off')

    # prepare the axes limits
    ax.set_xlim((-25, 25))
    ax.set_ylim((-35, 35))
    ax.set_zlim((5, 55))

    def lorenz_deriv(x_y_z, t0, sigma=sigma, beta=beta, rho=rho):
        """Compute the time-derivative of a Lorenz system."""
        x, y, z = x_y_z
        return [sigma * (y - x), x * (rho - z) - y, x * y - beta * z]

    # Choose random starting points, uniformly distributed from -15 to 15
    np.random.seed(1)
    x0 = -15 + 30 * np.random.random((N, 3))

    # Solve for the trajectories
    t = np.linspace(0, max_time, int(250*max_time))
    x_t = np.asarray([integrate.odeint(lorenz_deriv, x0i, t)
                      for x0i in x0])

    # choose a different color for each trajectory
    colors = plt.cm.viridis(np.linspace(0, 1, N))

    for i in range(N):
        x, y, z = x_t[i,:,:].T
        lines = ax.plot(x, y, z, '-', c=colors[i])
        plt.setp(lines, linewidth=2)

    ax.view_init(0, )
    plt.show()

    return t, x_t
```

```python
t, x_t = solve_lorenz(angle=180, N=10)
```

```python

```