from numpy import ndarray, eye, ones, array, zeros, float32, sqrt
from numpy.random import randint, seed
from System.Model.system import System

# change m to lambda
class SimpleHarmonicOscillator(System):
    def __init__(self, m: float, k: float, b: float, dt: float, H: ndarray, Q: ndarray, R: ndarray):
        self.m = m  # Mass
        k = k  # Spring constant
        b = b  # Damping coefficient
        self.dt = dt  # Time step

        super().__init__(
                        # State transition matrix (A)
                        Φ = array([[1, dt],
                                   [- (k * dt) / m, 1 - ((b / m) * dt)]]),

                        # Input transition matrix (B)
                        B = array([[0],
                                   [dt/m]]),
                        
                        H=H,
                        Q=Q,
                        R=R
        )

# Subclass SHM (Unkonwn M, unknown B, etc)