"""Calculation examples for SHARCON TECH lessons 11–15.

Install: python -m pip install numpy scipy
Run: python advanced-experiments.py
All inputs are synthetic. This file does not control equipment.
"""
import numpy as np
from scipy.integrate import solve_ivp
from scipy.linalg import expm


def identification():
    """Lesson 11: logarithmic R and L estimation from an RL transient."""
    voltage, final_current, initial_current = 4.0, 2.0, 0.0
    time = np.array([0.005, 0.010, 0.015, 0.020])
    current = np.array([0.78694, 1.26424, 1.55374, 1.72933])
    q = (final_current - current) / (final_current - initial_current)
    if not np.all((q > 0) & (q < 1)):
        raise ValueError("Logarithmic estimation requires 0 < q < 1")
    rate = -(time @ np.log(q)) / (time @ time)
    resistance = voltage / final_current
    inductance = resistance / rate
    print(f"11: R = {resistance:.5f} ohm; L = {inductance:.6f} H")
    np.testing.assert_allclose(inductance, 0.02, rtol=1e-4)


def pwm():
    """Lesson 12: exact period at fixed EMF and continuous current."""
    udc, resistance, inductance, emf = 24.0, 2.0, 0.02, 6.0
    frequency, duty = 2000.0, 10.4 / 24.0
    period, tau = 1 / frequency, inductance / resistance
    a = np.exp(-duty * period / tau)
    c = np.exp(-(1 - duty) * period / tau)
    ion, ioff = (udc - emf) / resistance, -emf / resistance
    imin = (ioff * (1 - c) + c * ion * (1 - a)) / (1 - a * c)
    imax = ion + (imin - ion) * a
    if imin <= 0:
        raise ValueError("A separate discontinuous-current model is required")
    returned = ioff + (imax - ioff) * c
    np.testing.assert_allclose(returned, imin, atol=1e-12)
    approx = udc * duty * (1 - duty) / (inductance * frequency)
    print(f"12: imin = {imin:.6f} A; imax = {imax:.6f} A")
    print(f"    peak-to-peak: exact {imax-imin:.6f} A; estimate {approx:.6f} A")


def cascade():
    """Lesson 13: tuning and linear poles without saturation or delay."""
    resistance, inductance, inertia, friction, kt = 2.0, 0.02, 0.002, 0.002, 0.1
    tau_i, zeta, wn = 0.005, 0.8, 10.0
    kpi, kii = inductance / tau_i, resistance / tau_i
    kpw = (2 * zeta * wn * inertia - friction) / kt
    kiw = inertia * wn**2 / kt
    polynomial = [inertia*tau_i, inertia+friction*tau_i,
                  friction+kt*kpw, kt*kiw]
    poles = np.roots(polynomial)
    assert np.all(poles.real < 0), "The linear cascade is unstable"
    print(f"13: Current PI: {kpi:.4f}, {kii:.4f}; Speed PI: {kpw:.4f}, {kiw:.4f}")
    print("    cascade poles:", poles)


def two_mass():
    """Lesson 14: free motion, angular momentum and energy dissipation."""
    j1, j2, stiffness, damping = 0.002, 0.008, 2.0, 0.008

    def rhs(time, x):
        w1, w2, theta, dissipated = x
        torque = stiffness*theta + damping*(w1-w2)
        return [-torque/j1, torque/j2, w1-w2, damping*(w1-w2)**2]

    time = np.linspace(0, 2, 4001)
    sol = solve_ivp(rhs, [0, 2], [0, 0, 0.05, 0], t_eval=time,
                    rtol=1e-9, atol=1e-11, max_step=0.002)
    if not sol.success:
        raise RuntimeError(sol.message)
    w1, w2, theta, dissipated = sol.y
    energy = (j1*w1**2 + j2*w2**2 + stiffness*theta**2) / 2
    residual = np.max(np.abs(energy + dissipated - energy[0]))
    momentum_error = np.max(np.abs(j1*w1 + j2*w2))
    assert residual < 1e-8, "Energy balance failed"
    assert momentum_error < 1e-10, "Angular momentum balance failed"
    assert np.max(np.diff(energy)) < 1e-9, "Energy increases"
    print(f"14: W(0) = {energy[0]:.7f} J; W(2) = {energy[-1]:.9f} J")
    print(f"    energy residual {residual:.2e} J; momentum {momentum_error:.2e}")


def digital():
    """Lesson 15: exact ZOH model and delay-induced stability changes."""
    resistance, inductance, sample_time = 2.0, 0.02, 0.001
    block = np.array([[-resistance/inductance, 1/inductance], [0.0, 0.0]])
    discrete = expm(block * sample_time)
    a, g = discrete[0, 0], discrete[0, 1]
    gain = 30.0
    no_delay = a - g*gain
    delayed = np.roots([1, -a, g*gain])
    np.testing.assert_allclose(a, np.exp(-sample_time*resistance/inductance))
    np.testing.assert_allclose(g, (1-a)/resistance)
    assert abs(no_delay) < 1 and np.all(np.abs(delayed) > 1)
    print(f"15: without delay z = {no_delay:.6f}; with delay: {delayed}")
    print(f"    K bounds: {(1+a)/g:.6f} and {1/g:.6f} V/A")


if __name__ == '__main__':
    for experiment in (identification, pwm, cascade, two_mass, digital):
        experiment()