
# do not edit, generated by pymola

from __future__ import print_function, division
import sympy
import sympy.physics.mechanics as mech
from pymola.sympy_runtime import OdeModel
from sympy import sin, cos, tan

class Quad(OdeModel):

    def __init__(self):

        super(Quad, self).__init__()

        # states
        x, y, z, R, Q, U, R, P, V, Q, P, W, phi, theta, psi, P, Q, R = mech.dynamicsymbols('x, y, z, R, Q, U, R, P, V, Q, P, W, phi, theta, psi, P, Q, R')
        self.x = sympy.Matrix([x, y, z, R, Q, U, R, P, V, Q, P, W, phi, theta, psi, P, Q, R])

        # inputs
        self.u = sympy.Matrix([])

        # outputs
        self.y = sympy.Matrix([])

        # constants
        self.c = sympy.Matrix([])
        self.c0 = {
            }

        # parameters
        J_x, J_y, J_z, m = sympy.symbols('J_x, J_y, J_z, m')
        self.p = sympy.Matrix([J_x, J_y, J_z, m])
        self.p0 = {
            'J_x' : 1,
            'J_y' : 1,
            'J_z' : 1,
            'm' : 1,
            }

        # variables
        F_y, F_z, F_x, M_z, M_y, M_x = sympy.symbols('F_y, F_z, F_x, M_z, M_y, M_x')
        self.v = sympy.Matrix([F_y, F_z, F_x, M_z, M_y, M_x])
      
        # equations
        self.eqs = [
            M_x - (- P - phi),
            M_y - (- Q - theta),
            M_z - (- R - psi),
            F_x - (- x),
            F_y - (- y),
            F_z - (- z),
            (x).diff(self.t) - (U),
            (y).diff(self.t) - (V),
            (z).diff(self.t) - (W),
            - m * V * (R).diff(self.t) + m * W * (Q).diff(self.t) + m * (U).diff(self.t) - (F_x),
            m * U * (R).diff(self.t) - m * W * (P).diff(self.t) + m * (V).diff(self.t) - (F_y),
            - m * U * (Q).diff(self.t) + m * V * (P).diff(self.t) + m * (W).diff(self.t) - (F_z),
            (phi).diff(self.t) - (P + Q * sin(phi) * tan(theta) + R * cos(phi) * tan(theta)),
            (theta).diff(self.t) - (Q * cos(phi) - R * sin(phi)),
            cos(theta) * (psi).diff(self.t) - (Q * sin(phi) + R * cos(phi)),
            (P).diff(self.t) - (M_x),
            (Q).diff(self.t) - (M_y),
            (R).diff(self.t) - (M_z),
            ]

        self.compute_fg()
    