
# 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
        phi, theta, psi_, P, Q, R, x, y, z, U, V, W = mech.dynamicsymbols('phi, theta, psi_, P, Q, R, x, y, z, U, V, W')
        self.x = sympy.Matrix([phi, theta, psi_, P, Q, R, x, y, z, U, V, W])
        self.x0 = {
            phi : 0,
            theta : 0,
            psi_ : 0,
            P : 0,
            Q : 0,
            R : 0,
            x : 0,
            y : 0,
            z : 0,
            U : 0,
            V : 0,
            W : 0,
            }

        # variables
        F_x, F_y, F_z, M_x, M_y, M_z = mech.dynamicsymbols('F_x, F_y, F_z, M_x, M_y, M_z')
        self.v = sympy.Matrix([F_x, F_y, F_z, M_x, M_y, M_z])

        # 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.0,
            J_y : 1.0,
            J_z : 1.0,
            m : 1.0,
            }

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

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

        # 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()