text:
module SixDOF

using ForwardDiff
using LinearAlgebra: norm, cross, eigen
using WriteVTK

export Control, MassProp, Reference
export AbstractAeroModel, AbstractPropulsionModel, AbstractInertialModel,
    AbstractAtmosphereModel, AbstractController
export StabilityDeriv, MotorPropBatteryDataFit, UniformGravitationalField,
    ConstantAtmosphere, ConstantController
export CO, COUNTER, COCOUNTER
export sixdof!
export finite_jacobian, geteig, getjacobian, animvtk
export Wing
#DEBUG
export aeroforces,propulsionforces,gravityforces, State

# ------ General Structs -------

struct Wing{TV, TF}
    pos::TV
    dir::TV
    cr::TF
    ct::TF
    tilt::TF
    twist::TF
end

"""
    State(x, y, z, phi, theta, psi, u, v, w, p, q, r)

State of the aircraft: positions in inertial frame, euler angles,
velocities in body frame, angular velocities in body frame.
"""
struct State{TF}
    x::TF  # position (inertial frame)
    y::TF
    z::TF
    phi::TF  # orientation, euler angles
    theta::TF
    psi::TF
    u::TF  # velocity (body frame)
    v::TF
    w::TF
    p::TF  # angular velocity (body frame)
    q::TF
    r::TF
end

"""
    Control(de, dr, da, df, throttle)

Define the control settings: delta elevator, delta rudder, delta aileron,
delta flaps, and throttle.
"""
struct Control{TF}
    de::TF  # elevator
    dr::TF  # rudder
    da::TF  # aileron
    df::TF  # rudder
    throttle::TF
end

"""
    MassProp(m, Ixx, Iyy, Izz, Ixz, Ixy, Iyz)

Mass and moments of inertia in the body frame.
Ixx = int(y^2 + z^2, dm)
Ixz = int(xz, dm)

Most aircraft are symmetric about y and so there is a convenience method
to specify only the nonzero components.
MassProp(m, Ixx, Iyy, Izz, Ixz)
"""
struct MassProp{TF}
    m::TF
    Ixx::TF
    Iyy::TF
    Izz::TF
    Ixz::TF
    Ixy::TF
    Iyz::TF
end

# most aircraft are symmetric in y
MassProp(m, Ixx, Iyy, Izz, Ixz) = MassProp(m, Ixx, Iyy, Izz, Ixz, zero(Ixx), zero(Ixx))

"""
    Reference(S, b, c)

The reference area, span, and chord used in the aerodynamic computations.
"""
struct Reference{TF}
    S::TF  # area
    b::TF  # span
    c::TF  # chord
end

# ----------------------------------------------

# --------------- Interfaces ---------------

abstract type AbstractAtmosphereModel end

"""
    wind(model::AbstractAtmosphereModel, state)

Compute wind velocities.

**Returns**
- Wi: wind velocities in inertial frame
- Wb: gust velocities in body frame (just a convenience to allow some velocities in body frame)
"""
function wind(model::AbstractAtmosphereModel, state)
    @warn "wind function not implemented for AbstractAtmosphereModel"
    Wi = [0.0, 0.0, 0.0]
    Wb = [0.0, 0.0, 0.0]
    return Wi, Wb
end

"""
    properties(model::AbstractAtmosphereModel, state)

Compute atmospheric density and the speed of sound.
"""
function properties(model::AbstractAtmosphereModel, state)
    @warn "properties function not implemented for AbstractAtmosphereModel"
    rho = 1.225  # sea-level properties
    asound = 340.3
    return rho, asound
end

"""
    gravity(model::AbstractAtmosphereModel, state)

Compute the local acceleration of gravity.
"""
function gravity(model::AbstractAtmosphereModel, state)
    @warn "gravity function not implemented for AbstractAtmosphereModel"
    g = 9.81
    return g
end

# ----

abstract type AbstractAeroModel end

"""
    aeroforces(model::AbstractAeroModel, atm::AbstractAtmosphereModel, state::State, control::Control, mp::MassProp, ref::Reference)

Compute the aerodynamic forces and moments in the body frame.
return F, M
"""
function aeroforces(model::AbstractAeroModel, atm, state, control, mp, ref)
    @warn "aeroforces function not implemented for AbstractAeroModel"
    # forces and moments in body frame
    F = [0.0, 0.0, 0.0]
    M = [0.0, 0.0, 0.0]
    return F, M
end

# ----

abstract type AbstractPropulsionModel end

"""
    propulsionforces(model::AbstractPropulsionModel, atm::AbstractAtmosphereModel, state::State, control::Control, mp::MassProp, ref::Reference)

Compute the propulsive forces and moments in the body frame.
return F, M
"""
function propulsionforces(model::AbstractPropulsionModel, atm, state, control, mp, ref)
    @warn "propulsionforces function not implemented for AbstractPropulsionModel"
    # forces and moments in body frame
    F = [0.0, 0.0, 0.0]
    M = [0.0, 0.0, 0.0]
    return F, M
end

# ----

abstract type AbstractInertialModel end

"""
    gravityforces(model::AbstractInertialModel, atm::AbstractAtmosphereModel, state::State, control::Control, mp::MassProp, ref::Reference)

Compute the gravitational forces and moments in the body frame.
return F, M
"""
function gravityforces(model::AbstractInertialModel, atm, state, control, mp, ref)
    @warn "gravityforces function not implemented for AbstractInertialModel"
    # forces and moments in body frame
    F = [0.0, 0.0, 0.0]
    M = [0.0, 0.0, 0.0]
    return F, M
end

# ----

abstract type AbstractController end

"""
    setcontrol(controller::AbstractController, time, atm::AbstractAtmosphereModel, state::State, lastcontrol::Control, mp::MassProp, ref::Reference)

Compute control state for next time step given current state
return control::Control
"""
function setcontrol(controller::AbstractController, time, atm, state, mp, ref)
    @warn "setcontrol function not implemented for AbstractController"
    control = Control(0.0, 0.0, 0.0, 0.0, 0.0)
    return control
end

# -----------------------------

# ------------- helper functions (private) --------------

"""
    inertialtobody(state)

Construct a rotation matrix from inertial frame to body frame

The assumed order of rotation is
1) psi radians about the z axis,
2) theta radians about the y axis,
3) phi radians about the x axis.

This is an orthogonal transformation so its inverse is its transpose.
"""
function inertialtobody(state)

    R = Array{eltype(state.phi)}(undef, 3, 3)

    cphi, ctht, cpsi = cos.([state.phi, state.theta, state.psi])
    sphi, stht, spsi = sin.([state.phi, state.theta, state.psi])

    R[1, 1] = ctht*cpsi
    R[1, 2] = ctht*spsi
    R[1, 3] = -stht

    R[2, 1] = sphi*stht*cpsi - cphi*spsi
    R[2, 2] = sphi*stht*spsi + cphi*cpsi
    R[2, 3] = sphi*ctht

    R[3, 1] = cphi*stht*cpsi + sphi*spsi
    R[3, 2] = cphi*stht*spsi - sphi*cpsi
    R[3, 3] = cphi*ctht

    return R

end

"""
    windtobody(alpha, beta)

Rotation matrix from wind frame to body frame.
- alpha: angle of attack
- beta: sideslip angle
"""
function windtobody(alpha, beta)

    ca, cb = cos.([alpha, beta])
    sa, sb = sin.([alpha, beta])

    Rwb = [ca*cb  -ca*sb  -sa;
           sb      cb     0.0;
           sa*cb  -sa*sb  ca]

    return Rwb
end

"""
    stabilitytobody(alpha, beta)

Rotation matrix from stability frame to body frame.
- alpha: angle of attack
"""
function stabilitytobody(alpha)

    ca = cos(alpha)
    sa = sin(alpha)

    Rsb = [ca  0.0  -sa;
           0.0 1.0  0.0;
           sa  0.0  ca]

    return Rsb
end

"""
    windaxes(atm::AbstractAtmosphereModel, state)

Compute relative velocity in wind axes (airspeed, aoa, sideslip)
"""
function windaxes(atm, state)

    # velocity vectors
    Vb = [state.u, state.v, state.w]
    Wi, Wb = wind(atm, state)

    Rib = inertialtobody(state)

    # relative wind
    Vrel = Vb - (Rib*Wi + Wb)

    # airspeed
    Va = norm(Vrel)

    # angle of attack
    alpha = atan(Vrel[3], Vrel[1])

    # sideslip
    beta = asin(Vrel[2] / Va)

    return Va, alpha, beta

end

# ----------------------------------------------------

# ------- Some Default Interface Implementations -----

"""
    StabilityDeriv(CL0, CLalpha, CLq, CLM, CLdf, CLde, alphas,
        CD0, U0, exp_Re, e, Mcc, CDdf, CDde, CDda, CDdr,
        CYbeta, CYp, CYr, CYda, CYdr, Clbeta,
        Clp, Clr, Clda, Cldr,
        Cm0, Cmalpha, Cmq, CmM, Cmdf, Cmde,
        Cnbeta, Cnp, Cnr, Cnda, Cndr)

Stability derivatives of the aircraft.  Most are self explanatory if you are
familiar with stability derivatives (e.g., CLalpha is dCL/dalpha or the lift curve slope).
Some less familiar ones include
- M: Mach number
- alphas: the angle of attack for stall
- U0: the speed for the reference Reynolds number CD0 was computed at
- exp_Re: the coefficient in the denominator of the skin friction coefficient (0.5 laminar, 0.2 turbulent)
- e: Oswald efficiency factor
- Mcc: crest critical Mach number (when compressibility drag rise starts)

"""
struct StabilityDeriv{TF} <: AbstractAeroModel
    CL0::TF
    CLalpha::TF
    CLq::TF
    CLM::TF
    CLdf::TF
    CLde::TF
    alphas::TF  # TODO: should probably do in terms of CLmax

    CD0::TF
    U0::TF  # velocity corresponding to Reynolds number of CD0  (TODO: rethink this)
    exp_Re::TF  # exponent for Reynolds number scaling. typical values: exp_Re = 0.5 laminar, 0.2 turbulent
    e::TF  # Oswald efficiency factor
    Mcc::TF  # crest-critical Mach number when compressibility drag rise starts (quartic)
    CDdf::TF
    CDde::TF
    CDda::TF
    CDdr::TF

    CYbeta::TF
    CYp::TF
    CYr::TF
    CYda::TF
    CYdr::TF

    Clbeta::TF
    Clp::TF
    Clr::TF
    Clda::TF
    Cldr::TF

    Cm0::TF
    Cmalpha::TF
    Cmq::TF
    CmM::TF
    Cmdf::TF
    Cmde::TF

    Cnbeta::TF
    Cnp::TF
    Cnr::TF
    Cnda::TF
    Cndr::TF
end

"""
A simple (mostly) linear aerodynamics model
"""
function aeroforces(sd::StabilityDeriv, atm, state, control, ref, mp)

    # airspeed, angle of attack, sideslip
    Va, alpha, beta = windaxes(atm, state)

    # Mach number and dynamic pressure
    rho, asound = properties(atm, state)
    Mach = Va / asound
    qdyn = 0.5 * rho * Va^2

    # rename for convenience
    p = state.p
    q = state.q
    r = state.r
    de = control.de
    df = control.df
    dr = control.dr
    da = control.da

    # lift
    CL = sd.CL0 + sd.CLalpha*alpha + sd.CLq*q *ref.c/(2*Va) + sd.CLM*Mach
        + sd.CLdf*df + sd.CLde*de

    em = exp(-50*(alpha - sd.alphas))
    ep = exp(50*(alpha + sd.alphas))
    sigma = (1 + em + ep)/((1 + em)*(1 + ep))
    CL = (1- sigma)*CL + sigma * 2 * sign(alpha)*sin(alpha)^2*cos(alpha)

    # drag
    CDp = sd.CD0*(Va/sd.U0)^sd.exp_Re
    CDi = CL^2/(pi*(ref.b^2/ref.S)*sd.e)
    CDc = (Mach < sd.Mcc) ? 0.0 : 20*(Mach - sd.Mcc)^4

    CD = CDp + CDi + CDc + abs(sd.CDdf*df) + abs(sd.CDde*de) + abs(sd.CDda*da) + abs(sd.CDdr*dr)

    # side force
    CY = sd.CYbeta*beta + (sd.CYp*p + sd.CYr*r)*ref.b/(2*Va) + sd.CYda*da + sd.CYdr*dr

    # rolling moment
    Cl = sd.Clbeta*beta + (sd.Clp*p + sd.Clr*r)*ref.b/(2*Va) + sd.Clda*da + sd.Cldr*dr

    # pitching moment
    Cm = sd.Cm0 + sd.Cmalpha*alpha + sd.Cmq*q * ref.c/(2*Va) + sd.CmM*Mach + sd.Cmdf*df + sd.Cmde*de

    # yawing moment
    Cn = sd.Cnbeta*beta + (sd.Cnp*p + sd.Cnr*r)*ref.b/(2*Va) + sd.Cnda*da + sd.Cndr*dr

    # transfer forces from wind to body axes
    Rwb = windtobody(alpha, beta)

    F = Rwb*[-CD, CY, -CL] * qdyn * ref.S

    M = Rwb*[Cl*ref.b, Cm*ref.c, Cn*ref.b] * qdyn * ref.S

    return F, M
end

@enum PropType CO=1 COUNTER=-1 COCOUNTER=0

"""
    MotorPropBatteryDataFit(CT2, CT1, CT0, CQ2, CQ1, CQ0, D, num, type,
        R, Kv, i0, voltage)

**Inputs**
- CT2, CT1, CT0: quadratic fit to propeller thrust coefficient of form: CT = CT2*J2 + CT1*J + CT0
- CQ2, CQ1, CQ0: quadratic fit to propeller torque coefficient of form: CQ = CQ2*J2 + CQ1*J + CQ0
- D: propeller diameter
- num: number of propellers
- type: CO (torques add), COUNTER (torques add but with minus sign), COCOUNTER (no torque, they cancel out)
- R: motor resistance
- Kv: motor Kv
- i0: motor no-load current
- voltage: battery voltage
"""
struct MotorPropBatteryDataFit{TF, TI, PropType} <: AbstractPropulsionModel
    # CT = CT2*J2 + CT1*J + CT0
    # CQ = CQ2*J2 + CQ1*J + CQ0
    CT2::TF  # prop data fit
    CT1::TF
    CT0::TF
    CQ2::TF
    CQ1::TF
    CQ0::TF
    D::TF  # prop diameter
    num::TI
    type::PropType

    R::TF  # motor resistance
    Kv::TF  # motor Kv
    i0::TF  # motor no-load current

    voltage::TF  # battery voltage
end

function propulsionforces(prop::MotorPropBatteryDataFit, atm, state, control, ref, mp)

    # airspeed, angle of attack, sideslip
    Va, _, _ = windaxes(atm, state)

    # density
    rho, _ = properties(atm, state)

    D = prop.D

    # determine torque for motor/prop match (quadratic equation)
    a = rho*D^5/(2*pi)^2 * prop.CQ0
    b = rho*D^4/(2*pi)*Va * prop.CQ1 + 1.0/(prop.R*prop.Kv)
    c = rho*D^3*Va^2 * prop.CQ2 - control.throttle*prop.voltage/(prop.R*prop.Kv) + prop.i0/prop.Kv
    Omega = (-b + sqrt(b^2 - 4*a*c))/(2*a)

    # advance ratio
    n = Omega/(2*pi)
    J = Va/(n*D)

    # thrust and torque
    CT = prop.CT0 + prop.CT1*J + prop.CT2*J^2
    CQ = prop.CQ0 + prop.CQ1*J + prop.CQ2*J^2

    T = prop.num * CT * rho * n^2 * D^4
    Q = prop.num * CQ * rho * n^2 * D^5 * Int(prop.type)

    return [T, 0, 0], [Q, 0, 0]
end

"""
    UniformGravitationalField()

Assumes center of mass and center of gravity are coincident.
"""
struct UniformGravitationalField <: AbstractInertialModel end

function gravityforces(model::UniformGravitationalField, atm, state, control, ref, mp)

    W = mp.m * gravity(atm, state)
    ct, cp = cos.([state.theta, state.phi])
    st, sp = sin.([state.theta, state.phi])

    Fg = W*[-st, ct*sp, ct*cp]
    Mg = [zero(W), zero(W), zero(W)]  # no gravitational moment

    return Fg, Mg
end

"""
    ConstantAtmosphere(Wi, Wb, rho, asound, g)

Constant atmospheric properties.
"""
struct ConstantAtmosphere{TF, TV<:AbstractVector{TF}} <: AbstractAtmosphereModel
    Wi::TV
    Wb::TV
    rho::TF
    asound::TF
    g::TF
end

function wind(atm::ConstantAtmosphere, state)
    return atm.Wi, atm.Wb
end

function properties(atm::ConstantAtmosphere, state)
    return atm.rho, atm.asound
end

function gravity(atm::ConstantAtmosphere, state)
    return atm.g
end

"""
    ConstantController(de, dr, da, df, throttle)

Just a dummy controller that outputs constant control outputs the whole time.
"""
struct ConstantController{TF} <: AbstractController
    de::TF
    dr::TF
    da::TF
    df::TF
    throttle::TF
end

function setcontrol(controller::ConstantController, time, atm, state, mp, ref)
    return Control(controller.de, controller.dr, controller.da, controller.df, controller.throttle)
end
# --------------------------------------------------------

# ------------- main functions (public) --------------

"""
    sixdof!(ds, s, params, time)

dynamic and kinematic ODEs.  Follows format used in DifferentialEquations package.
- s = x, y, z, phi, theta, psi, u, v, w, p, q, r (same order as State)
- params = control, massproperties, reference, aeromodel, propmodel, inertialmodel, atmmodel
"""
function sixdof!(ds, s, params, time)

    x, y, z, phi, theta, psi, u, v, w, p, q, r = s
    mp, ref, aeromodel, propmodel, inertialmodel, atmmodel, controller = params

    # ---- controller -------
    state = State(s...)
    control = setcontrol(controller, time, atmmodel, state, mp, ref)
    # -----------------------

    # --------- forces and moments ---------
    # aerodynamics
    Fa, Ma = aeroforces(aeromodel, atmmodel, state, control, ref, mp)

    # propulsion
    Fp, Mp = propulsionforces(propmodel, atmmodel, state, control, ref, mp)

    # weight
    Fg, Mg = gravityforces(inertialmodel, atmmodel, state, control, ref, mp)

    # total forces and moments
    F = Fa + Fp + Fg
    M = Ma + Mp + Mg

    # --------------------------------------

    # ----- derivative of state --------
    Vb = [u, v, w]
    omegab = [p, q, r]

    # linear kinematics
    Rib = inertialtobody(state)
    rdot = Rib' * Vb

    # angular kinematics
    phidot = p + (q*sin(phi) + r*cos(phi))*tan(theta)
    thetadot = q*cos(phi) - r*sin(phi)
    psidot = (q*sin(phi) + r*cos(phi))/cos(theta)

    # linear dynamics
    vdot = F/mp.m - cross(omegab, Vb)

    # angular dyna1mics
    I = [mp.Ixx -mp.Ixy -mp.Ixz;
         -mp.Iyz mp.Iyy -mp.Iyz;
         -mp.Ixz -mp.Iyz mp.Izz]
    omegadot = I \ (M - cross(omegab, I*omegab))

    # -------------------------

    # TODO: if we need more efficiency we can avoid allocating and then assigning.
    ds[1:3] = rdot
    #ds[4:6] = omegab #Use for quaternions??
    ds[4] = phidot
    ds[5] = thetadot
    ds[6] = psidot
    ds[7:9] = vdot
    ds[10:12] = omegadot
end

function multQuat(a,b)#a and b are quaternions (four element vectors) being multiplied such that
    #result = a*b
    w = a[1]*b[1] - b[2]*a[2] - b[3]*a[3] - b[4]*a[4]
    x = b[1]*a[2] + b[2]*a[1] - b[3]*a[4] + b[4]*a[3]
    y = b[1]*a[3] + b[2]*a[4] + b[3]*a[1] - b[4]*a[2]
    z = b[1]*a[4] - b[2]*a[3] + b[3]*a[2] + b[4]*a[1]
    out = [w,x,y,z]
    return out
end

#Rotates a quaternion (quat) that represents a rigid body.
#Rotation is based on the angular velocity (omeg) and the time step (dt).
function rotQuat(p,omeg,dt) #(4-tuple,3-tuple,double)
    #result = a rotated quaternion representing the rigid body one dt later.

    #for use later
    oriMag= norm(p)

    #find axis of rotation and angle to rotate through.
    angSpd= norm(omeg)#angular speed about dir

    if angSpd == 0.0 #AVOID NaNs
        dir= omeg
    else
        dir= omeg ./ angSpd #Axis of rotation
    end

    th= angSpd*dt#theta (angular displacement) about the dir axis

    #create quaternions for rotation
    th = th/2.0 #quaternions double the angle input to them
    q= [  cos(th), sin(th)*dir[1], sin(th)*dir[2], sin(th)*dir[3]  ]#the rotation quaternion
    qinv= [  cos(-th), sin(-th)*dir[1], sin(-th)*dir[2], sin(-th)*dir[3]  ]#other rotation quaternion

    #make output
    out= multQuat(multQuat(q,p),qinv)
    finalMag= norm(out) #Make sure magnitude is 1

    if finalMag != 0
        out = out.*(oriMag/finalMag)#DONT NORMALIZE THE QUATERNION!! Make the output have the same magnitude as before.
    end

    return out
end

function eulerToQuat(phi,th,psi)#takes euler angles. Gives axis and angle.
    #Define quaternions for the whole rotation.
    phi = phi/2.0
    th = th/2.0
    psi = psi/2.0

    q1= [cos(psi),0,0,sin(psi)]
    yb= rotQuat([0.0,0.0,1.0,0.0],[0.0,0.0,1.0],psi)#body y
    x1= rotQuat([0.0,1.0,0.0,0.0],[0.0,0.0,1.0],psi)#intermediate
    q2= [cos(th),(yb[2:4].*sin(th))...]
    xb= rotQuat(x1,yb[2:4],th)#body x
    q3= [cos(phi),(xb[2:4].*sin(phi))...]

    #Define one quaternion for the rotation.
    qout= multQuat(multQuat(q3,q2),q1)
    magQ= norm(qout) #Make sure magnitude is 1
    if magQ==0.0
        qout = qout
    else
        qout = qout./magQ
    end
    #extract axis and angle from qout
    angle = acos(qout[1])*2
    magQ=norm(qout[2:4])
    if magQ==0.0
        axis = qout[2:4]
    else
        axis = qout[2:4]./magQ
    end

    return axis, angle
end

"""

    finite_jacobian(s, params)

    Calculates the jacobian matrix of partial derivatives for the 6-DOF model
    represented by s and params.

**Inputs**
- s = x, y, z, phi, theta, psi, u, v, w, p, q, r (same order as State)
- params = control, massproperties, reference, aeromodel, propmodel, inertialmodel, atmmodel

**Outputs**
- jacobian: a jacobian matrix representing the system.

"""

function finite_jacobian(state,params)

    #How much to step (perturb) each state value. The closer this is to zero, the more theoretically accurate.
    per=0.000001

    #Allocate ds. This makes them the same size and type. This value will be overwritten
    ds = zeros(length(state))
    ds[1:3]=state[7:9]
    #required for use of sixdof!() Not implemented.
    time = 10.0
    #Also must be reset each time sixdof!() is used.
    centerS=state
    centerP=params
    sixdof!(ds, centerS, centerP, time)#sixdof!() modifies ds so that it is now the defailt derivatives
    #Result: values in ds give the derivatives about the center point.
    centerds=ds

    #initialize the Jacobian matrix with zeros.
    jacobian=zeros(length(state),length(state))

    #Loop to perturb each element of state and build the jacobian matrix
    for i in 1:length(state)
        #Must be reset each time sixdof!() is used to ensure that we are measuring around the same point each time.
        time = 0.0
        centerS=zeros(length(state))
        centerS=state
        centerP=params
        ds = zeros(length(state))
        ds[1:3]=state[7:9]

        #Perturb s at correct index
        centerS[i]=centerS[i]+per
        #Run sixdof!() for perturbed value.
        sixdof!(ds, centerS, centerP, time)

        #Calculate the derivitives relative to centerds
        deltaS = ds - centerds

        #Store the new vector in its respective column.
        jacobian[:,i]=deltaS./per

        centerS[i]=centerS[i]-per#This may fix a weird bug
    end

    return jacobian
end

function getjacobian(s,params)
    ds = zeros(length(s))
    function centered_sixdof!(ds, s)#used exclusivley for functionality with ForwardDiff.jl
        t = 0.0
        sixdof!(ds,s,params,t)
    end
    jacobian = ForwardDiff.jacobian(centered_sixdof!, ds, s)
    return jacobian
end

"""

    geteig(s, params)

    Calculates the eigenvalues and eigenvectors for the 6-DOF model
    represented by s and params.

**Inputs**
- s = x, y, z, phi, theta, psi, u, v, w, p, q, r (same order as State)
- params = control, massproperties, reference, aeromodel, propmodel, inertialmodel, atmmodel

**Outputs**
- eigVal: Eigenvalues representing the system.
- eigVect: Eigenvectors of the system.

"""
function geteig(s, params)
    #get the jacobian matrix
    jacobian=finite_jacobian(s,params)

    eigVal, eigVect = eigen(jacobian)

    return eigVal, eigVect#eigVal(i) corresponds to eigVect(:,i)
end

"""

    animvtk(jacobian, model)

    Makes an animation of stability modes based on the jacobian matrix and a model aircraft.

**Inputs**
- jacobian: jacobian matrix for the system.
- model: Vector of arbitrary length. Each element contains info about an aerodynamic surface.
- an element of model = Wing(position, direction,  cr, ct, tilt, twist) (dir has enough info for length, sweep and dihedral)

**Outputs**
- Writes a file:

"""
function animvtk(jacobian, model, planeName)#Makes a VTK animation
    #TODO
    targetamp=pi/6#30 deg max

    #TODO: determine these two in a more robust way
    resolution = 100.0
    simEnd = 0.5

    #We need these for the animation.
    eigVal, eigVect = eigen(jacobian)
    numEig=length(eigVal)
    surfs=length(model)#Number of surfaces in the model.

    #Define necessary points for zero displacement and zero angular displacement.
    #They will be quaternions so that they can be moved easily.
    numPts=4*surfs+1
    quatPoints= zeros(4,numPts)              #4 points for each surface plus one for COM
    for i in 1:surfs                         #define all points, one surface at a time
        tilt = (pi/180)*model[i].tilt        #convert to radians
        twist = (pi/180)*model[i].twist
        quatPoints[2:4,4*i-2]= model[i].pos + [cos(tilt) 0.0 sin(tilt)].*model[i].cr/2               #Root Front
        quatPoints[2:4,4*i-1]= model[i].pos + model[i].dir + [cos(tilt+twist) 0.0 sin(tilt+twist)].*model[i].ct/2  #Tip Front
        quatPoints[2:4,  4*i]= model[i].pos - [cos(tilt) 0.0 sin(tilt)].*model[i].cr/2                             #Root Rear
        quatPoints[2:4,4*i+1]= model[i].pos + model[i].dir - [cos(tilt+twist) 0.0 sin(tilt+twist)].*model[i].cr/2  #Tip rear
    end
    rotQuatPoints=zeros(4,numPts)           #allocate
    rotQuatPoints[:,1:numPts] = quatPoints  #initialize

    #DEFINE POLYGONS
    polys= [MeshCell(PolyData.Polys(), (4*i-2):(4*i)) for i = 1:surfs]
    polys2= [MeshCell(PolyData.Polys(), (4*i-1):(4*i+1)) for i = 1:surfs]
    append!(polys,polys2)

    #CLEAR EXISTING Anim FOLDER
    rm("Anim/$planeName",recursive=true,force=true)
    mkdir("Anim/$planeName")
    mkdir("Anim/$planeName/vtk")
    numPer=0             #Counter for periodic modes
    numSub=0             #Counter for stable modes
    numUn=0              #Counter for unstable modes
    numPerSub=0          #Counter for stable periodic modes
    numPerUn=0           #Counter for unstable periodic modes

    for mode in 1:numEig #Loop for all animations

        x = eigVect[:,mode]  #get eigenvector
        lam = eigVal[mode]   #get eigenvalue
        if imag(lam)<0.0     #Modes with negative imaginary parts are duplicates
            @goto animEnd
        end

        #Label to identify what type of mode it is. Initialized as " "
        type = " "

        #initialize
        tau=1.0/real(lam)             #time constant
        TT=2.0*pi/imag(lam)           #oscilation period
        #Determine simulation length and amplitude
        if tau==NaN || tau==Inf   #Means pure periodic mode
            if TT==NaN || TT==Inf #Means Stationary mode
                simEnd=1.0
                amp=0.0
                type="Stationary"
                @goto animEnd     #Stationary modes aren't interesting
            else
                simEnd=TT          #Simulate for 1 period (if periodic)
                amp=targetamp/norm(x[4:6])
                type="Periodic"
                numPer=numPer+1
                filename="Anim/$planeName"*"/$type"*"_$numPer"
            end
        elseif TT==NaN  || TT==Inf           #Means pure exponential
            if tau<0.0                       #tau is positive (stable mode)
                simEnd=-3.0*tau              #simulate for 3 time constants
                amp=targetamp/norm(x[4:6])
                type="Subsidence"
                numSub=numSub+1
                filename="Anim/$planeName"*"/$type"*"_$numSub"
            else                            #tau is positive (unstable mode)
                simEnd=tau                  #simulate 1 time constant
                #Magnitude at end of simulation is biggest, so use it as a baseline.
                amp=targetamp/(norm(x[4:6])*real(exp(lam*simEnd)))
                type="Unstable"
                numUn=numUn+1
                filename="Anim/$planeName"*"/$type"*"_$numUn"
            end
        else                      #means exponential and periodic
            if tau<0.0            #Means decay
                simEnd=-5.0*tau   #simulate for 5 time constants
                amp=targetamp/(norm(x[4:6]))
                type="Periodic_Subsidence"
                numPerSub=numPerSub+1
                filename="Anim/$planeName"*"/$type"*"_$numPerSub"
            else                     #tau is positive (unstable mode)
                simEnd=5.0*tau       #simulate 5 time constants
                #Magnitude at end of simulation is biggest, so use it as a baseline.
                amp=2*targetamp/(norm(x[4:6])*exp(real(lam*simEnd)))
                type="Periodic_Unstable"
                numPerUn=numPerUn+1
                filename="Anim/$planeName"*"/$type"*"_$numPerUn"
            end
        end
        step=simEnd/resolution
        if amp==NaN || amp==Inf#Catch NaNs so they dont become a problem.
            amp=0.0
        end

        #ANIMATE EACH MODE
        animPVD = paraview_collection(filename*".pvd")#WARNING: Probably only works on mac and linux due to structure of "filename."

        #ADD VTK FILES TO .pvd COLLECTION. ONE VTK FILE AT A TIME
        for t in 0.0:step:simEnd

            #define state at any time, look at only real part
            xt = amp*real(x.*exp(lam*t))

            #get axis and angle from euler angles.
            rotAxis,rotAngle =eulerToQuat(xt[4:6]...)

            #rotate graphics
            for j in 1:numPts
                rotQuatPoints[:,j] = rotQuat(quatPoints[:,j],rotAxis,rotAngle)
            end

            pts = rotQuatPoints[2:4, : ]#points array (non-quaternion) just for animation
            vtkFrame = vtk_grid("Anim/$planeName"*"/vtk/mode_"*"$planeName"*"_$mode"*"__t_$t"*".vtp",pts,polys)#one vtk file
            vtkFrame["time"] = t
            vtkFrame["λreal"] = real(lam)
            vtkFrame["λimag"] = imag(lam)
            vtkFrame["τ"] = tau
            vtkFrame["T"] = TT
            vtkFrame["A"] = amp
            vtkFrame["x_x"] = real(x[1])
            vtkFrame["x_y"] = real(x[2])
            vtkFrame["x_z"] = real(x[3])
            vtkFrame["x_Φ"] = real(x[4])
            vtkFrame["x_Θ"] = real(x[5])
            vtkFrame["x_Ψ"] = real(x[6])
            vtkFrame["x_Vx"] = real(x[7])
            vtkFrame["x_Vy"] = real(x[8])
            vtkFrame["x_Vz"] = real(x[9])
            vtkFrame["x_p"] = real(x[10])
            vtkFrame["x_q"] = real(x[11])
            vtkFrame["x_r"] = real(x[12])

            animPVD[t]= vtkFrame        #paraview animation
        end
        vtk_save(animPVD)               #save the paraview animation

        #ANIMATE EACH PERIODIC MODE AGAIN, SHOWING ONLY PERIODIC PARTS
        if type=="Periodic_Unstable" || type=="Periodic_Subsidence"
            animPVD2 = paraview_collection(filename*"_Periodic"*".pvd")#WARNING: Probably only works on mac and linux
            simEnd=2*pi          #Simulate for 1 period
            step=simEnd/resolution
            amp=targetamp/norm(x[4:6])

            #ADD VTK FILES TO .pvd COLLECTION. ONE VTK FILE AT A TIME
            for t in 0.0:step:simEnd
                xt = amp*real(x).*sin(t) #define at any time, look at only real part of periodic response.

                #get axis and angle from euler angles.
                rotAxis,rotAngle =eulerToQuat(xt[4:6]...)

                #rotate graphics
                for j in 1:numPts
                    rotQuatPoints[:,j] = rotQuat(quatPoints[:,j],rotAxis,rotAngle)
                end

                pts = rotQuatPoints[2:4, : ]#points array (non-quaternion) just for animation
                vtkFrame = vtk_grid("Anim/$planeName"*"/vtk/mode_"*"$planeName"*"_$mode"*"_Periodic__t_$t"*".vtp",pts,polys)#one vtk file
                vtkFrame["time"] = t
                realLam = real(lam) #TODO Allocate these earlier to avoid doing it every loop.
                imagLam = imag(lam)
                vtkFrame["λ"] = "$realLam"*" + $imagLam"*"i"
                vtkFrame["τ"] = tau
                vtkFrame["T"] = TT
                vtkFrame["A"] = amp
                animPVD2[t*TT/(2*pi)]= vtkFrame#paraview animation
            end
            vtk_save(animPVD2)#save the paraview animation

            @label animEnd #stationary modes will skip down to here
        end

    end
end

function trimstate()
end

end # module


text:
@enum Namespace biological_process=1 cellular_component=2 molecular_function=3 external=4 gene_ontology=5

mutable struct GOTerm
    id::Int
    name::String
    namespace::Namespace
    definition::String

    alt_id::Vector{Int}
    is_a::Vector{Int}

    is_obsolete::Bool
    replaced_by::Int
    consider::Vector{Int}

    part_of::Vector{Int}
    regulates::Vector{Int}
    positively_regulates::Vector{Int}
    negatively_regulates::Vector{Int}

    GOTerm() = new(0, "", gene_ontology, "", Int[], Int[], true, 0, Int[], Int[], Int[], Int[], Int[])
end

showgoset(gots) = show(IOContext(stdout, :compact=>true), gots)

function Base.show(io::IO, got::GOTerm)
    if get(io, :compact, false)
        print(io, "\n")
        print(io, num_to_goid(got.id))
        print(io, "\t", got.namespace)
        print(io, "\t", got.name)

    else
        println(io, "GO Term")
        println(io, "id                   : ", num_to_goid(got.id))
        println(io, "name                 : ", got.name)
        println(io, "namespace            : ", got.namespace)
        println(io, "definition           : ", got.definition)
        println(io, "alt_id               : ", got.alt_id)
        println(io, "part_of              : ", got.part_of)
        println(io, "is_a                 : ", got.is_a)
        println(io, "is_obsolete          : ", got.is_obsolete)
        println(io, "replaced_by          : ", got.replaced_by)
        println(io, "consider             : ", got.consider)
        println(io, "regulates            : ", got.regulates)
        println(io, "positively_regulates : ", got.positively_regulates)
        println(io, "negatively_regulates : ", got.negatively_regulates)
    end
end

function parse_line(line, go_term, conds)
    for (prefix, parse_fun, field, ignore) in conds
        if startswith(line, prefix)
            if !ignore
                v = parse_fun(line[(length(prefix) + 1):end])
                if typeof(getfield(go_term, field)) <: AbstractVector
                    push!(getfield(go_term, field), v)
                else
                    try
                        setfield!(go_term, field, v)
                    catch e
                        println("Error setting: $field with $v using $parse_fun")
                        display(field)
                        display(v)
                        throw(e)
                    end
                end
            end
            return
        end
    end

    println("field missing: #$line#")
    return
end

@inline validid(go_terms, id) = haskey(go_terms, id) ? go_terms[id].id : -1
@inline isaltid(go_terms, id) = haskey(go_terms, id) && (go_terms[id].id != id)
@inline goid_to_num(id::AbstractString) = parse(Int, id[4:end])::Int
num_to_goid(num::Int) = @sprintf("GO:%07d", num)
function parse_namespace(namespace)
    if     namespace == "biological_process"
            return biological_process
    elseif namespace == "cellular_component"
            return cellular_component
    elseif namespace == "molecular_function"
            return molecular_function
    elseif namespace == "external"
            return external
    elseif namespace == "gene_ontology"
            return gene_ontology
    else
        error("Namespace parse error: $namespace")
    end
end

parse_annot_id(annot_id) = goid_to_num(annot_id[1:10])
parse_is_obsolete(is_obs) = parse(Bool, is_obs)

function get_parse_conditions()

    conds = Vector{Tuple{String, Function, Symbol, Bool}}()

    # cond tuple (prefix, parse_fun, field, ignore)
    push!(conds, ("id: ",          goid_to_num,       :id,          false))
    push!(conds, ("name: ",        identity,          :name,        false))
    push!(conds, ("namespace: ",   parse_namespace,   :namespace,   false))
    push!(conds, ("def: ",         identity,          :definition,  false))
    push!(conds, ("alt_id: ",      goid_to_num,       :alt_id,      false))
    push!(conds, ("is_a: ",        parse_annot_id,    :is_a,        false))
    push!(conds, ("is_obsolete: ", parse_is_obsolete, :is_obsolete, false))
    push!(conds, ("replaced_by: ", goid_to_num,       :replaced_by, false))
    push!(conds, ("consider: ",    goid_to_num,       :consider,    false))

    ### relationshsips
    push!(conds, ("relationship: part_of ",              parse_annot_id, :part_of,              false))
    push!(conds, ("relationship: regulates " ,            parse_annot_id, :regulates,            false))
    push!(conds, ("relationship: positively_regulates ", parse_annot_id, :positively_regulates, false))
    push!(conds, ("relationship: negatively_regulates ", parse_annot_id, :negatively_regulates, false))

    ## ignore fields
    push!(conds, ("synonym: ",  identity, :ignore, true))
    push!(conds, ("xref: ",     identity, :ignore, true))
    push!(conds, ("subset: ",   identity, :ignore, true))
    push!(conds, ("comment: ",  identity, :ignore, true))

end

function parseontology(file)
    ids = Vector{Int}()

    reading_term = false
    reading_typedef = false

    go_terms = Dict{Int, GOTerm}()
    go_term = GOTerm()

    conds = get_parse_conditions()

    for line in eachline(file)
        isempty(line) && continue
        if line == "[Term]"

            reading_term && addterm!(go_terms, go_term)
            reading_term = true
            go_term = GOTerm()
        elseif line == "[Typedef]"
            reading_term && addterm!(go_terms, go_term)
            reading_term = false
        elseif reading_term
            parse_line(line, go_term, conds)
        end
    end
    reading_term && addterm!(go_terms, go_term)
    go_terms
end

function addterm!(go_terms, go_term)
    go_terms[go_term.id] = go_term
    for gt in go_term.alt_id
        if haskey(go_terms, gt)
            println("Overwriting error:")
            display(go_terms[gt])
            display(go_term)
            error("Overwrite")
        end
        go_terms[gt] = go_term
    end
end

ext: jl


text:
module Celerite2

include("terms.jl")
include("core.jl")
include("gp.jl")

end # module


text:
module CSTAnalyzer

const loaded_mods = Dict{String,Tuple{Set{Symbol},Set{Symbol}}}()
using CSTParser
using AbstractTrees
using ..CSTUtils
const BaseCoreNames = Set(vcat(names(Base), names(Core), :end, :new, :ccall))

abstract type Walker; end

mutable struct File
    path::String
    parent::Union{Nothing,Tuple{File,UnitRange{Int}}}
    children::Vector{File}
end

mutable struct Location{T}
    path::String
    offset::T
end

mutable struct Binding
    t
    loc::Location{UnitRange{Int}}
    val
end

mutable struct Scope
    t::String
    namespace::String
    parent::Union{Nothing,Scope}
    children::Vector{Scope}
    names::Dict{String,Vector{Binding}}
    range::UnitRange{Int64}
    loc::Location
end
AbstractTrees.children(s::Scope) = s.children
Scope() = Scope("__toplevel__", "", nothing, [], Dict(), 1:typemax(Int), Location("", 1:typemax(Int)))
function Base.show(io::IO, s::Scope, i = 0)
    println(io, " "^i, s.t, ":[", join(keys(s.names), ", "), "]")
    for c in s.children
        show(io, c, i + 1)
    end
end

struct MissingBinding
    s::Scope
end
mutable struct Reference{T}
    x::T
    b::Union{Binding,MissingBinding}
    loc::Location{UnitRange{Int}}
end

function Base.show(io::IO, ref::Reference{T}) where T
    print(io, "Reference{$T}(", ref.x, "...)")
end

struct FileSystem end
mutable struct State{T}
    current_scope::Scope
    loc::Location
    target_file::String
    bad_refs::Vector{Reference}
    refs::Vector{Reference}
    nodecl::UnitRange{Int}
    isquotenode::Bool
    includes::Dict
    fs::T
end
State(s) = State{FileSystem}(s, Location("", 0), "", [], [], 0:0, false, Dict(), FileSystem())
State() = State(Scope())
function Base.show(io::IO, s::State)
    print(io, "State(<target file ")
    show(s.target_file)
    println(io, ">)")
end

function add_binding(x, name, t, S::State, offset)
    if haskey(S.current_scope.names, name)
        push!(S.current_scope.names[name], Binding(t, Location(S.loc.path, offset), x))
    else
        S.current_scope.names[name] = Binding[Binding(t, Location(S.loc.path, offset), x)]
    end
end

function add_scope(a, s, S::State, t, name = "")
    push!(S.current_scope.children, Scope(t, name, s, [], Dict(), S.loc.offset + a.span, Location(s.loc.path, S.loc.offset + a.span)))
    S.current_scope = last(S.current_scope.children)
end

function create_scope(x, s, S::State)
    if CSTParser.defines_module(x)
        add_scope(x, s, S, "Module", CSTParser.str_value(CSTParser.get_name(x)))
    elseif CSTParser.defines_function(x)
        add_scope(x, s, S, "Function")
        sig = CSTParser.get_sig(x)
        for param in CSTParser.get_sig_params(sig)
            add_binding(sig, param, :DataType, S, S.loc.offset + x.span)
        end
        for arg in CSTParser.get_args(x)
            add_binding(arg, CSTParser.str_value(arg), :Any, S, S.loc.offset + x.span)
        end
    elseif CSTParser.defines_macro(x)
        add_scope(x, s, S, "Macro")
        for arg in CSTParser.get_args(x)
            add_binding(arg, CSTParser.str_value(arg), :Any, S, S.loc.offset + x.args[1].fullspan + x.args[2].span)
        end
    elseif CSTParser.defines_datatype(x)
        add_scope(x, s, S, string(typeof(x).parameters[1].name.name))
        if x isa CSTParser.EXPR{CSTParser.Struct} || x isa CSTParser.EXPR{CSTParser.Mutable}
            sig = CSTParser.get_sig(x)
            sig = CSTParser.rem_subtype(sig)
            sig = CSTParser.rem_where(sig)
            for arg in CSTParser.get_curly_params(sig)
                add_binding(sig, arg, :Any, S, S.loc.offset + x.args[1].fullspan + x.args[1].span)
            end

            for arg in CSTParser.get_args(x)
                add_binding(arg, CSTParser.str_value(arg), :Any, S, S.loc.offset + x.span)
            end
        end
    elseif x isa CSTParser.EXPR{CSTParser.Let}
        add_scope(x, s, S, "Let")
    elseif x isa CSTParser.EXPR{CSTParser.Quote}
        add_scope(x, s, S, "Quote")
    elseif x isa CSTParser.EXPR{CSTParser.Do}
        add_scope(x, s, S, "Do")
        for arg in CSTParser.get_args(x)
            add_binding(arg, CSTParser.str_value(arg), :Any, S, S.loc.offset + x.args[1].fullspan + x.args[2].fullspan + x.args[3].span)
        end
    elseif x isa CSTParser.EXPR{CSTParser.While}
        add_scope(x, s, S, "While")
    elseif x isa CSTParser.EXPR{CSTParser.For}
        add_scope(x, s, S, "For")
        S.nodecl = S.loc.offset + x.args[1].fullspan + x.args[2].span
        if x.args[2] isa CSTParser.EXPR{CSTParser.Block}
            offset1 = S.loc.offset + x.args[1].fullspan
            for iter in x.args[2]
                if CSTParser.is_valid_iterator(iter)
                    if iter.arg1 isa CSTParser.IDENTIFIER || iter.arg1 isa CSTParser.EXPR{CSTParser.TupleH}
                        for ass in CSTParser.flatten_tuple(iter.arg1)
                            add_binding(ass, CSTParser.str_value(ass), :Any, S, offset1 + iter.span)
                        end
                    end
                end
                offset1 += iter.fullspan
            end
        elseif CSTParser.is_valid_iterator(x.args[2])
            offset1 = S.loc.offset + x.args[1].fullspan
            if x.args[2].arg1 isa CSTParser.IDENTIFIER || x.args[2].arg1 isa CSTParser.EXPR{CSTParser.TupleH}
                for ass in CSTParser.flatten_tuple(x.args[2].arg1)
                    add_binding(ass, CSTParser.str_value(ass), :Any, S, offset1 + x.args[2].span)
                end
            end
        end
    elseif x isa CSTParser.EXPR{CSTParser.Try}
        add_scope(x, s, S, "Try")
    elseif x isa CSTParser.WhereOpCall
        add_scope(x, s, S, "WhereOpCall")
        for arg in CSTParser.get_where_params(x)
            add_binding(x, arg, :DataType, S, S.loc.offset + x.span)
        end
    elseif x isa CSTParser.EXPR{CSTParser.Generator}
        add_scope(x, s, S, "Generator")
        for arg in CSTParser.get_args(x)
            add_binding(arg, CSTParser.str_value(arg), :Any, S, S.loc.offset + x.args[1].fullspan + x.args[2].fullspan + x.args[3].span)
        end
    elseif CSTParser.defines_anon_function(x)
        add_scope(x, s, S, "anon_func")
        for arg in CSTParser.get_args(x)
            add_binding(arg, CSTParser.str_value(arg), :Any, S, S.loc.offset + x.span)
        end
    elseif CSTParser.is_assignment(x) && x.arg1 isa CSTParser.EXPR{CSTParser.Curly}
        add_scope(x, s, S, "Typealias")
        for param in CSTParser.get_curly_params(x.arg1)
            add_binding(x, param, :Any, S, S.loc.offset + x.span)
        end
    elseif isexpr(x, CSTParser.MacroCall)
        if isexpr(children(x)[1], CSTParser.MacroName) && length(children(children(x)[1])) == 2 &&
                id_name(x.args[1].args[2]) == "eval"
            add_scope(x, s, S, "@eval")
        elseif isexpr(children(x)[1], CSTParser.GlobalRefDoc)
            doced_expr = children(x)[3]
            # If the doc'ed expr is a call, it is treated as a signature.
            # The argument names should have argument scope
            if isexpr(doced_expr, CSTParser.Call)
                offset1 = S.loc.offset + children(x)[2].fullspan
                add_scope(x, s, S, "docsig")
                for arg in CSTParser.get_args(doced_expr)
                    add_binding(arg, CSTParser.str_value(arg), :Any, S, offset1 + doced_expr.span)
                end
            end
        end
    end
end

function get_rhs(x)
    if CSTParser.is_assignment(x.arg2)
        return get_rhs(x.arg2)
    else
        return x.arg2
    end
end

function get_external_binding(x, s, S::State)
    if (S.loc.offset + x.span) in S.nodecl
        return false
    elseif CSTParser.defines_function(x)
        name = CSTParser.str_value(CSTParser.get_name(x))
        add_binding(x, name, :Function, S, S.loc.offset + x.span)
        return true
    elseif CSTParser.defines_macro(x)
        name = CSTParser.str_value(CSTParser.get_name(x))
        add_binding(x, name, :Macro, S, S.loc.offset + x.span)
        return true
    elseif CSTParser.defines_datatype(x)
        name = CSTParser.str_value(CSTParser.get_name(x))
        add_binding(x, name, :DataType, S, S.loc.offset + x.span)
        return true
    elseif CSTParser.defines_module(x)
        name = CSTParser.str_value(CSTParser.get_name(x))
        add_binding(x, name, :Module, S, S.loc.offset + x.span)
        return false
    elseif CSTParser.is_assignment(x)
        ass = x.arg1
        ass = CSTParser.rem_decl(ass)
        ass = CSTParser.rem_curly(ass)
        if ass isa CSTParser.IDENTIFIER
            add_binding(x, CSTParser.str_value(ass), :Any, S, S.loc.offset + x.span)
        elseif ass isa CSTParser.EXPR{CSTParser.TupleH}
            for arg in CSTParser.flatten_tuple(ass)
                add_binding(x, CSTParser.str_value(arg), :Any, S, S.loc.offset + x.span)
            end
        end
        return true
    elseif x isa CSTParser.EXPR{CSTParser.Local} && !CSTParser.is_assignment(x.args[1])
        if x.args[2] isa CSTParser.IDENTIFIER
            add_binding(x, CSTParser.str_value(x.args[2]), :Any, S, S.loc.offset + x.span)
        else
            for arg in x.args[2]
                if arg isa CSTParser.IDENTIFIER
                    add_binding(arg, CSTParser.str_value(arg), :Any, S, S.loc.offset + x.span)
                end
            end
        end
        return true
    elseif x isa CSTParser.EXPR{CSTParser.MacroCall}
        if x.args[1] isa CSTParser.EXPR{CSTParser.MacroName} && length(x.args[1].args) == 2 && CSTParser.str_value(x.args[1].args[2]) == "enum"
            enum_name = ""
            for i = 2:length(x.args)
                arg = x.args[i]
                if arg isa CSTParser.IDENTIFIER
                    if isempty(enum_name)
                        enum_name = CSTParser.str_value(arg)
                        add_binding(x, enum_name, :DataType, S, S.loc.offset + x.span)
                    else
                        add_binding(x, CSTParser.str_value(arg), Symbol(enum_name), S, S.loc.offset + x.span)
                    end
                end
            end
        end
        return true
    elseif x isa CSTParser.EXPR{T} where T <: Union{CSTParser.Import,CSTParser.Using,CSTParser.ImportAll}
        get_imports(x, S)
        return true
    end
    return false
end

find_ref(name, S::State) = find_ref(name, S.current_scope, S)

function find_ref(name, s, S::State)
    if name in keys(s.names)
        return last(s.names[name])
    elseif s.parent != nothing
        return find_ref(name, s.parent, S)
    else
        if Symbol(name) in BaseCoreNames
            return Binding("BaseCore", Location("____", 0:0), nothing)
        else
            return MissingBinding(S.current_scope)
        end
    end
end

# Find scopes
function find_scope(s::Scope, range::UnitRange)
    if !(range in s.range)
        error("")
    end
    for child in children(s)
        if range in child.range && child.loc.path == s.loc.path
            return find_scope(child, range)
        end
    end
    return s
end
find_scope(S::State, range::UnitRange{Int}) =
    find_scope(S.current_scope, range)

# Build scopes

function lint_call(w::Walker, x, s, S) end
function lint_call(w::Walker, x::CSTParser.EXPR{CSTParser.Call}, s, S)
    fname = CSTParser.get_name(x)
    if CSTParser.str_value(fname) == "include"
        follow_include(x, s, S)
    end
end

Base.in(x::UnitRange{Int}, y::UnitRange{Int}) = first(x) ≥ first(y) && last(x) ≤ last(y)

function find_bad_refs(S)
    for r in S.refs
        if r.b isa MissingBinding
            resolved = resolve_missing_binding(r)
            if !resolved
                push!(S.bad_refs, r)
            end
        end
    end
end

function resolve_missing_binding(r::Reference, s = r.b.s)
    if CSTParser.str_value(r.x) in keys(s.names)
        r.b = last(s.names[CSTParser.str_value(r.x)])
        return true
    elseif s.parent != nothing
        return resolve_missing_binding(r, s.parent)
    end
    return false
end

include("trav.jl")
include("imports.jl")
include("includes.jl")

function resolve(S::State, file_scope::Scope, node::OverlayNode{CSTParser.IDENTIFIER})
    scope = find_scope(file_scope, node.span)
    find_ref(span_text(node), scope, S)
end

end # module


text:
using Jeff, MCMCChains, Turing, Distributions, DelimitedFiles, DataFrames, Optim, TuringCallbacks, Turkie, Makie

layers = Array{Any}(undef, (4, 4))
layers[1, 1] = 0.0
layers[1, 2] = 2.07
layers[1, 3] = 0.0
layers[1, 4] = 0
layers[2, 1] = 38
layers[2, 2] = 3.47
layers[2, 3] = 0.
layers[2, 4] = 5.86
layers[3, 1] = 258.827
layers[3, 2] = 2.4
layers[3, 3] = 0.0
layers[3, 4] = 9.0
layers[4, 1] = 0
layers[4, 2] = 6.36
layers[4, 3] = 0.0
layers[4, 4] = 3.68

scale = 0.8797
bkg = 4.4265e-7

data = readdlm("/Users/andrewmccluskey/work/analysis/Jeff.jl/dev/c_PLP0011859_q.txt")

x = data[:, 1]
y = data[:, 2]
yerr = data[:, 3]

function log_likelihood(ll, scale, bkg, x, y, yerr)
  layers[2, 1] = ll
	model = Jeff.constant_smearing(x, layers, 5., scale, bkg)
  sigma2 = yerr.^2.0
  return -0.5 .* sum((y .- model).^2 ./ sigma2 .+ log.(sigma2))
end

my_loglike = (ll, scale, bkg) -> log_likelihood(ll, scale, bkg, x, y, yerr)

@model function my_model()
  #layers[2, 1] ~ Uniform(15, 50)
  #layers[2, 4] ~ Uniform(1., 15.)
  #layers[3, 1] ~ Uniform(200., 300.)
  #layers[3, 2] ~ Uniform(1.5, 3.5)
  #layers[3, 4] ~ Uniform(1., 15.)
  #layers[4, 4] ~ Uniform(1., 15.)
  ll ~ Uniform(15, 50)
  scale ~ Uniform(0.6, 1.2)
  bkg ~ Uniform(1e-9, 9e-6)
  Turing.@addlogprob! my_loglike(ll, scale, bkg)
end

num_adapts = 250
mod = my_model()

callback = TensorBoardCallback("tensorboard_logs/run")
alg = NUTS(num_adapts, 0.65)
chain = sample(mod, alg, 2500; callback=callback)

display(chain)
#plot(chain)
#savefig("distribtions.pdf")

df = DataFrame(chain)

#p0 = plot(x, y, yerror=yerr, label="data", yaxis=:log10, markerstrokecolor=:auto, ylim=(2e-7, 2))
#for val in eachrow(df[1:5:end, [:"bkg", :"layers[2,1]", :"layers[2,4]", :"layers[3,1]", :"layers[3,2]", :"layers[3,4]", :"layers[4,4]", :"scale"]])
#     layers[2, 1] = val[2]
#     layers[2, 4] = val[3]
#     layers[3, 1] = val[4]
#     layers[3, 2] = val[5]
#     layers[3, 4] = val[6]
#     layers[4, 4] = val[7]
#     plot!(p0, x, Jeff.constant_smearing(x, layers, 5., val[8], val[1]), label="", linealpha=0.01, linecolor=:black)
#end
#savefig("reflectometry.pdf")


text:
using LatticeVis

lattice = Lattice(Honeycomb, 10)
plot(lattice)

using PyPlot

lattice = Lattice(Honeycomb, 10, do_periodic = false)

figure(figsize=(10, 10))
plot(lattice)
xlim(-5, 21)
ylim(1, 27)


text:
using SparseArrays, LinearAlgebra
# generate a random pos def matrix with eigenvalues between 0.1 and 2
function generate_pos_def_matrix(rng::MersenneTwister, n::Int64, aMin::Real = 0.1, aMax::Real = 2)
	X = rand(rng, n, n)
	# any real square matrix can be QP decomposed into a orthogonal matrix and an uppertriangular matrix R
	Q, R = qr(X)
	eigs = rand(rng ,n) .* (aMax .- aMin) .+ aMin
	X = Q * Matrix(Diagonal(eigs)) * Q'
	X = 0.5 * (X + X')
	return X
end

function is_numerically_pos_sem_def(X, atol)
	X = X ./ 2
	X = X + X'

	F = eigfact(X)
	if size(find( x-> x < -atol, F[:values]), 1) == 0
		return true
	else
		return false
	end
end

function is_numerically_symmetric(X,atol)
	n = size(X, 2)
	for i = 1:n-1, j = i+1:n
		if abs(X[i, j] - X[j, i]) >= atol
			return false
		end
	end
	return true
end

function find_nonsymmetric_component(X)
	for i = 2:size(X, 1), j = 1:(i - 1)
		if abs(X[i, j] - X[j, i]) > 0.0
			return i, j, abs(X[i, j] - X[j, i])
		end
	end
end

function find_different_elements(A, B)
	if size(A) != size(B)
		error("Matrices are not the same size")
	end
	m, n = size(A)
	diff_el = Array[]
	for iii = 1:m, jjj = 1:n
		if A[iii, jjj] != B[iii, jjj]
			push!(diff_el, [iii, jjj])
		end
	end
	return diff_el
end

function duplicate_sparsity_pattern(A)
	m, n = size(A)
	B = zeros(m, n)
	for iii = 1:m, jjj = 1:n
		if A[iii, jjj] != 0
			B[iii, jjj] = 1
		end
	end
	return B
end

function apply_pattern!(A::AbstractMatrix, pattern::AbstractMatrix)
  @assert size(A) == size(pattern) "Matrix A and pattern must have same dimensions."
  m, n = size(A)
  for i = 1:m, j = 1:n
    if pattern[i, j] == 0
      A[i, j] = 0.
    end
  end
end

# create a feasible SDP with one PSDConeTriangle constraint
# min c' x
# s.t. At x + s == bt
#       s ∈ PSDConeTriangle
# choose At and bt in such a way that S has the provided sparsity pattern
function feasible_sdp_with_pattern(rng::MersenneTwister, pattern::AbstractMatrix)
  n = size(pattern, 1)
  d = div(n * (n + 1), 2)

  S = generate_pos_def_matrix(rng, n, 0.1, 2)
  apply_pattern!(S, pattern)
  S = Symmetric(S, :U)

  A1 = rand(rng, n, n)
  apply_pattern!(A1, pattern)
  A1 = Symmetric(A1, :U)
  A = hcat(A1[:])
  s = S[:]
  x = rand(rng, 1)
  b = A * x + s
  B = reshape(b, n, n)

  Y = generate_pos_def_matrix(rng, n,  0.1, 1)
  y = vec(Y)
  P = sparse(zeros(1, 1))
  q = -P * x - A' * y

  Ct = [COSMO.PsdConeTriangle(d)];
  At = zeros(d)
  bt = zeros(d)
  COSMO.extract_upper_triangle!(A1, At, sqrt(2))
  COSMO.extract_upper_triangle!(B, bt, sqrt(2))
  At = hcat(At)
  return P, q, At, bt, Ct
end

"Take a vector `svec` representing the `d` upper-triangular entries of a matrix `X`, and return `X`."
function matrixify(svec::AbstractVector)
  n = - 0.5 + sqrt(0.25 + 2 * length(svec))
  n = Int64(n)
  X = zeros(n, n)
  COSMO.populate_upper_triangle!(X, svec, 1 /sqrt(2))
  return Symmetric(X, :U)
end

function recreate_sparse_matrix(A::SparseMatrixCSC)
	rowInd = A.rowval
	colPtr = A.colptr
	val = A.nzval

	#compute column indices
	colInd = zeros(Int64, length(rowInd))
	cval = 1
	for iii = 2:length(colPtr)
		currentPtr = colPtr[iii]
		prevPtr = colPtr[iii - 1]
		colInd[prevPtr:currentPtr - 1] = cval
		cval += 1
	end

	# sort rowInd and vals
	p = sortperm(rowInd)
	return sparse(rowInd, colInd, val, size(A, 1), size(A, 2))
end

# Geometric mean from https://github.com/JuliaStats/StatsBase.jl
function gmean(a::AbstractArray{T}) where T<:Real
	s = 0.0
	n = length(a)
	for i in 1:n
		tmp = a[i]
		if tmp < 0.0
			throw(DomainError())
		elseif tmp == 0.0
			return 0.0
		else
			s += log(tmp)
		end
	end
	return exp(s / n)
end


text:
include("constants.jl")

# schemes
cpl=1;
moist=1;

#boundary conditions
TAup=270; #top model temperature
UAup=(0.2/karman).*(log((ZAF[end]+ΔZA[end]/2)/1e-4)); #top model temperature
UOdown=0; #lower boundary model current

# numerics
Aimp=0.5; # Atmospheric implicitness
Oimp=0.5; # Oceanic implicitness


text:
using Base.Test

using BayesianNonparametricStatistics, Distributions, PDMats

srand(123)

@testset "BayesianNonparametricStatistics package" begin
    @testset "model.jl" begin
        @test supertype(SDEModel) == AbstractModel
        @test supertype(AbstractModel) == Any
        for σ in -2.0:0.1:2.0
            for x in -2.0:0.1:2.0
                M = SDEModel(σ, x)
                @test M.σ == σ
                @test M.beginvalue == x
            end
        end
        σ = identity
        M = SDEModel(σ, 0.0)
        @test M.σ == identity
        σ(x) = 1
        M = SDEModel(σ, 0.0)
        @test M.σ == σ

        σ(x) = 2+sin(x)
        M = SDEModel(σ, 1.3)
        @test M.σ == σ
        @test M.beginvalue == 1.3
    end

    @testset "samplepath.jl" begin
        @test supertype(SamplePath) == AbstractSamplePath

        @test supertype(AbstractSamplePath) == Any

        X = SamplePath(0.0:0.1:1.0, sin)
        @test X.timeinterval == 0.0:0.1:1.0
        @test X.samplevalues == sin.(0.0:0.1:1.0)

        X = SamplePath(0.0:0.1:1.0, 1.0:11.0)

        @test X.timeinterval == 0.0:0.1:1.0
        @test X.samplevalues == 1.0:11.0

        X = SamplePath(0.0:0.1:1.0, x->2*x)
        @test X.timeinterval == 0.0:0.1:1.0
        @test X.samplevalues == collect(0.0:0.2:2.0)

        @test step(X) == 0.1
        @test length(X) == 11

        X = SamplePath([1., 2.], [1.1, 2.1])
        @test X.timeinterval == [1., 2.]
        @test X.samplevalues == [1.1, 2.1]

        @test length(X) == 2

        X = SamplePath([1.0, 3.0, 5.0, 9.0], x->x^2)
        @test X.timeinterval == [1.0, 3.0, 5.0, 9.0]
        @test X.samplevalues == [1.0, 9.0, 25.0, 81.0]

        X = SamplePath(collect(-2.0:0.01:2.0), sin)
        X.timeinterval == collect(-2.0:0.01:2.0)
        X.samplevalues == sin.(collect(-2.0:0.01:2.0))

        X = SamplePath(-2.0:0.01:2.0, sin)
        X.timeinterval == collect(-2.0:0.01:2.0)
        X.samplevalues == sin.(collect(-2.0:0.01:2.0))

        X = SamplePath(0.0:0.1:2π, sin)
        @test length(X) == length(0.0:0.1:2π)

        @test_throws ErrorException SamplePath(0.0:2.0, [1.0, 2.0])

        @test_throws ErrorException SamplePath(0.0:-0.1:-0.1, [0.0, 2.0])

        @test_throws ErrorException SamplePath([0.1, 2.0],[3.0])

        @test_throws ErrorException SamplePath([0.1, 2.0, 1.0], [1.0,2.0,3.0])

        @test BayesianNonparametricStatistics.isincreasing(0:0.1:1)
        @test !BayesianNonparametricStatistics.isincreasing(0:-0.1:-1.0)
        @test !BayesianNonparametricStatistics.isincreasing([0.0, 0.1, 0.2, 0.1, 0.3, 0.4])
        @test !BayesianNonparametricStatistics.isincreasing([0.0, -0.1, 0.0, 0.2, 0.3])
        @test !BayesianNonparametricStatistics.isincreasing([1, 2, 3, 4, -10, 11, 12])
        @test BayesianNonparametricStatistics.isincreasing([1, 2, 3, 4, 500, 600, 2000, 10000])
        @test BayesianNonparametricStatistics.isincreasing([1., 2., 2.00000001, 2.00000002])
        @test !BayesianNonparametricStatistics.isincreasing([2.,2.,2.,2.,2.])
        @test !BayesianNonparametricStatistics.isincreasing([2.,2., 3.0, 3.0001])
    end

    @testset "basisfunctions.jl" begin
        @test_throws ErrorException fourier(0)
        @test_throws ErrorException fourier(-1)
        @test_throws ErrorException fourier(-10)

        # The Fourier functions are orthogonal.
        # n = 3
        # X = SamplePath(0.:.01:1.0, 0.:.01:1.0)
        # for k in 1:n-1
        #     for l in k+1:n
        #         @test abs(BayesianNonparametricStatistics.calculateLebesgueintegral(x->fourier(k)(x)*fourier(l)(x), X))<0.1
        #     end
        # end
        # # And the square integrates to one.
        # for k in 1:n
        #     @test abs(BayesianNonparametricStatistics.calculateLebesgueintegral(x->fourier(k)(x)^2,X)-1.) <0.1
        # end
        #
        # They are one-periodic.
        n = 3
        x = 0.0:0.1:0.9
        for k in 1:n
            for y in x
                value = fourier(k)(y)
                for m in -10.0:10.0
                    valuetwo = fourier(k)(y+m)
                    @test value ≈ valuetwo
                end
            end
        end

        # They have minimum -sqrt(2) and maximum sqrt(2).
        n = 3
        x = 0.0:0.01:1.0
        for k in 1:n
            values = map(fourier(k), x)
            @test abs(minimum(values)+sqrt(2.0)) < 0.1
            @test abs(maximum(values)-sqrt(2.0)) < 0.1
        end

        @test_throws ErrorException faberschauder(-1, 1)
        @test_throws ErrorException faberschauder(-3, 1)
        @test_throws ErrorException faberschauder(1, 0)
        @test_throws ErrorException faberschauder(1, -2)
        @test_throws ErrorException faberschauder(1, 3)

        # Below we test Faber-Schauder functions on their mathematical properities, up
        # to level n, defined below.
        n = 2
        X=SamplePath(0.:0.001:1.0,0.:0.001:1.0)

        # Faber-Schauder functions are level-wise orthogonal, for levels j≥1.
        for j in 1:n
            for k in 1:2^j-1
                for l in k+1:2^j
                    @test abs(BayesianNonparametricStatistics.calculateLebesgueintegral(x->faberschauder(j,k)(x)*faberschauder(j,l)(x), X))<0.01
                end
            end
        end

        # They integrate to 2^(-j-1)
        @test abs(BayesianNonparametricStatistics.calculateLebesgueintegral(faberschauderone, X)-0.5) <0.01
        for j in 1:n
            for k in 1:2^j
                @test abs(BayesianNonparametricStatistics.calculateLebesgueintegral(faberschauder(j,k), X)- 2.0^(-j-1))<0.01
            end
        end

        # They have minimum 0 and maximum 1.
        x = 0.0:0.001:1.0
        y = map(faberschauderone, x)
        @test abs(minimum(y)) < 0.01
        @test abs(maximum(y)-1.0) < 0.01

        for j in 1:n
            for k in 1:2^j
                y = map(faberschauder(j,k), x)
                @test abs(minimum(y)) < 0.01
                @test abs(maximum(y)-1.0) < 0.01
            end
        end

        # They are one-periodic.
        x = 0.0:0.01:0.99
        for y in x
            value = faberschauderone(y)
            for k in -10.0:10.0
                valuetwo = faberschauderone(y+k)
                @test value≈valuetwo
            end
        end

        for j in 0:n
            for k in 1:2^j
                for y in x
                    value = faberschauder(j,k)(y)
                    for m in -10.:10.
                        valuetwo = faberschauder(j,k)(y+m)
                        @test value≈valuetwo
                    end
                end
            end
        end
        # End of tests for mathematical properities of Faber-Schauder functions.

    end

    @testset "SDE.jl" begin
        @test Set(subtypes(AbstractSDE)) == Set([SDE, SDEWithConstantVariance])

        @test supertype(AbstractSDE) == Any

        @test_throws ErrorException SDE(x->0.0, x->1.0, 0.0, 10.0, -0.1)

        @test_throws ErrorException SDE(x->0.0, x->1.0, 0.0, -1.0, 0.1)

        # The following should represent a Brownian motion.
        sde_SDE_type = SDE(x->0.0, x->1.0, 0, 1, 0.001)

        lengthvector = 1000
        x = Vector{Float64}(lengthvector)

        for k in 1:lengthvector
            X = rand(sde_SDE_type)
            x[k] = X.samplevalues[end]
        end

        @test abs(mean(x)) < 0.1
        @test 0.9 < var(x) < 1.1

        X = rand(sde_SDE_type)

        @test length(X) == length(0.0:sde_SDE_type.dt:sde_SDE_type.endtime)
        @test X.timeinterval == 0.0:sde_SDE_type.dt:sde_SDE_type.endtime

        geometricbrownianmotion = SDE(x->0.5*x, identity, 1, 1, 0.001)

        for k in 1:lengthvector
            X = rand(geometricbrownianmotion)
            x[k] = X.samplevalues[end]
        end

        @test abs(e^(0.5)-mean(x)) < 0.1

        X = rand(geometricbrownianmotion)

        @test length(X) == length(0.0:geometricbrownianmotion.dt:geometricbrownianmotion.endtime)
        @test X.timeinterval ==  0.0:geometricbrownianmotion.dt:geometricbrownianmotion.endtime

        @test_throws ErrorException SDEWithConstantVariance(x->0.0, 1.0, 0.0,
            10.0, -0.1)

        @test_throws ErrorException SDEWithConstantVariance(x->0.0, 1.0, 0.0,
            -1.0, 0.1)

        sde_SDEWithConstantVariance_type = SDEWithConstantVariance(x->0, 1, 0, 1, 0.001)

        for k in 1:lengthvector
            X = rand(sde_SDEWithConstantVariance_type)
            x[k] = X.samplevalues[end]
        end

        @test abs(mean(x)) < 0.1
        @test 0.9 < var(x) < 1.1

        X = rand(sde_SDEWithConstantVariance_type)
        @test X.timeinterval == 0.0:sde_SDEWithConstantVariance_type.dt:sde_SDEWithConstantVariance_type.endtime
        @test length(X) == length(0.0:sde_SDEWithConstantVariance_type.dt:sde_SDEWithConstantVariance_type.endtime)

        ornsteinuhlenbeckprocess = SDEWithConstantVariance(x-> -x, 1, e, 1, 0.001)

        for k in 1:lengthvector
            X = rand(ornsteinuhlenbeckprocess)
            x[k] = X.samplevalues[end]
        end

        @test abs(mean(x) - 1) < 0.1

        X = rand(ornsteinuhlenbeckprocess)

        @test X.timeinterval == 0.0:ornsteinuhlenbeckprocess.dt:ornsteinuhlenbeckprocess.endtime
        @test length(X) == length(0.0:ornsteinuhlenbeckprocess.dt:ornsteinuhlenbeckprocess.endtime)
    end

    @testset "gaussianprocess.jl" begin
        @test Set(subtypes(AbstractGaussianProcess)) ==
        Set([GaussianProcess, FaberSchauderExpansionWithGaussianCoefficients])

        @test supertype(AbstractGaussianProcess) == Any

        @test_throws ErrorException GaussianProcess([fourier(k) for k in 1:2],
            MvNormal([1.0,1.0,1.0]))

        X = GaussianProcess([sin, cos], MvNormal([1.0, 1.0]))

        @test X.basis == [sin, cos]

        @test typeof(X.distribution) == MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Distributions.ZeroVector{Float64}}
        @test X.distribution.Σ.diag == [1.0, 1.0]
        @test mean(X.distribution) == [0.0,0.0]
        @test length(X) == 2
        @test typeof(rand(X)) <: Function

        n = 10000
        a = sinpi(1/4)
        x = Vector{Float64}(n)

        for k in 1:n
            f = rand(X)
            x[k] = f(π/4)
        end

        @test abs(mean(x)) < 0.01
        @test abs(var(x)-2*a^2) < 0.1

        X = GaussianProcess([sinpi, cospi], MvNormal([1.0,1.0], [1.0,1.0]))

        for k in 1:n
            f = rand(X)
            x[k] = f(1/4)
        end

        @test abs(mean(x)-2*a)<0.01
        @test abs(var(x)-2*a^2)<0.1

        x = 0.0:0.1:1.0
        y = sumoffunctions([sin, cos], [1., 1.]).(x)
        z = map(x-> sin(x)+cos(x), x)
        @test y ≈ z

        # Test for two cases whether calculatedependencystructurefaberschauderbasis
        # calculates the right dependency structure of the Faber-Schauder basis.
        A = [
        true true true true true true true true;
        true true true true true true true true;
        true true true false true true false false;
        true true false true false false true true;
        true true true false true false false false;
        true true true false false true false false;
        true true false true false false true false;
        true true false true false false false true
        ]

        A = Symmetric(A)

        @test calculatedependencystructurefaberschauderbasis(2) == A

        A = [
        true true true true true true true true true true true true true true true true; #1
        true true true true true true true true true true true true true true true true; #2
        true true true false true true false false true true true true false false false false; #3
        true true false true false false true true false false false false true true true true; #4
        true true true false true false false false true true false false false false false false; #5
        true true true false false true false false false false true true false false false false; #6
        true true false true false false true false false false false false true true false false; #7
        true true false true false false false true false false false false false false true true; #8
        true true true false true false false false true false false false false false false false; #9
        true true true false true false false false false true false false false false false false; #10
        true true true false false true false false false false true false false false false false; #11
        true true true false false true false false false false false true false false false false; #12
        true true false true false false true false false false false false true false false false; #13
        true true false true false false true false false false false false false true false false; #14
        true true false true false false false true false false false false false false true false; #15
        true true false true false false false true false false false false false false false true #16
        ]

        A = Symmetric(A)

        @test calculatedependencystructurefaberschauderbasis(3) == A

        A = PDSparseMat(sparse([1.0 0.0; 0.0 1.0]))

        distribution = MvNormalCanon(A)

        @test_throws ErrorException FaberSchauderExpansionWithGaussianCoefficients(1,distribution)

        @test_throws ErrorException FaberSchauderExpansionWithGaussianCoefficients(Vector{Float64}(0))

        Π = FaberSchauderExpansionWithGaussianCoefficients(0,distribution)
        @test length(Π) == 2

        @test_throws ErrorException sumoffunctions([sin], [1.0,2.0])

        n = 10000
        x = Vector{Float64}(n)

        for k in 1:n
            f = rand(Π)
            x[k] = f(0.5)
        end

        @test abs(mean(x))<0.1
        @test abs(var(x)-1.0) <0.1

        for k in 1:n
            f = rand(Π)
            x[k] = f(0.0)
        end

        @test abs(mean(x))<0.1
        @test abs(var(x)-1.0) <0.1

        for k in 1:n
            f = rand(Π)
            x[k] = f(1.0)
        end

        @test abs(mean(x))<0.1
        @test abs(var(x)-1.0) <0.1

        distribution = MvNormalCanon([1.0, -2.0],[1.0, -2.0], A)
        Π = FaberSchauderExpansionWithGaussianCoefficients(0,distribution)

        @test length(Π) == 2

        n = 10000
        x = Vector{Float64}(n)

        for k in 1:n
            f = rand(Π)
            x[k] = f(0.5)
        end

        @test abs(mean(x)+2.0)<0.1
        @test abs(var(x)-1.0) <0.1

        for k in 1:n
            f = rand(Π)
            x[k] = f(0.0)
        end

        @test abs(mean(x)-1.0)<0.1
        @test abs(var(x)-1.0) <0.1

        for k in 1:n
            f = rand(Π)
            x[k] = f(1.0)
        end

        @test abs(mean(x)-1.0)<0.1
        @test abs(var(x)-1.0) <0.1

        Π = FaberSchauderExpansionWithGaussianCoefficients([1.0, 0.5])

        n = 10000
        x = Vector{Float64}(n)
        for k in 1:n
            f = rand(Π)
            x[k] = f(0.25)
        end

        @test abs(mean(x)) < 0.1
        @test abs(var(x) - 2.5) < 0.1

        A = PDSparseMat(sparse(eye(4)))

        distribution = MvNormalCanon([1.0,1.0,1.0,1.0], [1.0,1.0,1.0,1.0], A)

        Π = FaberSchauderExpansionWithGaussianCoefficients(1, distribution)

        n = 10000
        x = Vector{Float64}(n)
        for k in 1:n
            f = rand(Π)
            x[k] = f(0.25)
        end

        @test abs(mean(x)-2.0) < 0.1
        @test abs(var(x)-1.5) < 0.1

        X = SamplePath(0.0:10.0, [1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,11.])
        @test BayesianNonparametricStatistics.calculatestochasticintegral(identity, X) ≈ 55.0
        @test BayesianNonparametricStatistics.calculateLebesgueintegral(x->1., X) ≈ 10.0

        X = SamplePath(collect(0.0:10.0), [1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,11.])
        @test BayesianNonparametricStatistics.calculatestochasticintegral(identity, X) ≈ 55.0
        @test BayesianNonparametricStatistics.calculateLebesgueintegral(x->1., X) ≈ 10.0

        α = 0.5
        Π = FaberSchauderExpansionWithGaussianCoefficients([2^(α*j) for j in 1:5])

        sde = SDEWithConstantVariance(x->0.0, 1.0, 0.0, 10000.0, 0.01)
        X = rand(sde)

        M = SDEModel(1.0, 0.0)

        postΠ = calculateposterior(Π, X, M)

        n = 1000
        y = 0.0:0.01:1.0
        x = Array{Float64}(length(y),n)
        for k in 1:n
            f = rand(postΠ)
            x[:,k] = f.(y)
        end

        @test maximum(abs.(mean(x,1))) < 0.1

        M = SDEModel(1.0, 0.0)

        Π = GaussianProcess([fourier(k) for k in 1:40], MvNormal([k^(-1.0) for k in 1:40]))

        postΠ = calculateposterior(Π, X, M)

        n = 1000
        y = 0.0:0.01:1.0
        x = Array{Float64}(length(y),n)
        for k in 1:n
            f = rand(postΠ)
            x[:,k] = f.(y)
        end

        @test maximum(abs.(mean(x,1))) < 0.1

        M = SDEModel(x->1.0, 0.0)

        Π = GaussianProcess([fourier(k) for k in 1:40], MvNormal([k^(-1.0) for k in 1:40]))

        postΠ = calculateposterior(Π, X, M)

        n = 1000
        y = 0.0:0.01:1.0
        x = Array{Float64}(length(y),n)
        for k in 1:n
            f = rand(postΠ)
            x[:,k] = f.(y)
        end

        @test maximum(abs.(mean(x,1))) < 0.1

        M = SDEModel(x->1.0, 0.0)

        Π = FaberSchauderExpansionWithGaussianCoefficients([2^(α*j) for j in 1:5])

        postΠ = calculateposterior(Π, X, M)

        n = 1000
        y = 0.0:0.01:1.0
        x = Array{Float64}(length(y),n)
        for k in 1:n
            f = rand(postΠ)
            x[:,k] = f.(y)
        end

        @test maximum(abs.(mean(x,1))) < 0.1
    end
end


text:
using MathLink
sphericalbesselj_M(n, x) = weval(W"SphericalBesselJ"(W"n", W"x"); n, x)

@testset "_bessels.jl" begin
    @testset "domain" begin
        @test_throws DomainError SparseIR.sphericalbesselj(-2, 0.3)
        @test !isnan(SparseIR.sphericalbesselj(4, 1e20))
    end

    @testset "accuracy of sphericalbesselj(2^$pn, 2^$px)" for pn in 0:7, px in -1:25
        n = 2^pn
        x = 2.0^px
        @test SparseIR.sphericalbesselj(n, x) ≈ sphericalbesselj_M(n, x)
        @test SparseIR.sphericalbesselj(n + 1, x) ≈ sphericalbesselj_M(n + 1, x)
    end
end


text:
export ISMRMRDFile, AcquisitionHeaderImmutable, EncodingCountersImmutable

include("HDF5Types.jl")
include("Parameters.jl")

struct ISMRMRDFile <: MRIFile
  filename::String
end

"""
    RawAcquisitionData(f::ISMRMRDFile, dataset="dataset")

reads the `ISMRMRDFile` f and stores the result in a `RawAcquisitionDataObject`
"""
function MRIBase.RawAcquisitionData(f::ISMRMRDFile, dataset="dataset")

  h5open(f.filename) do h
    headerStr = read(h["/$(dataset)/xml"])
    params = GeneralParameters(headerStr[1])

    d = read(h["/$(dataset)/data"])
    M = length(d)
    profiles = Profile[]

    for m=1:M
      head = read_header(d[m].head)
      D = Int(head.trajectory_dimensions)
      chan = Int(head.active_channels)

      traj = isempty(d[m].traj) ? Matrix{Float32}(undef,0,0) : reshape(d[m].traj, D, :)

      if !isempty(d[m].data)
        dat = reshape(reinterpret(ComplexF32,d[m].data), :, chan)
      else
        dat = Matrix{ComplexF32}(undef,0,0)
      end

      push!(profiles, Profile(head,traj,dat) )
    end
    return RawAcquisitionData(params, profiles)
  end
end

"""
    AcquisitionData(f::ISMRMRDFile, dataset="dataset")

reads the `ISMRMRDFile` f and stores the result in an `AcquisitionDataObject`
"""
function MRIBase.AcquisitionData(f::ISMRMRDFile, dataset="dataset")
  return AcquisitionData(RawAcquisitionData(f,dataset))
end

function read_header(h)

  idx = EncodingCounters(h.idx.kspace_encode_step_1, h.idx.kspace_encode_step_2,
                      h.idx.average, h.idx.slice, h.idx.contrast, h.idx.phase, h.idx.repetition,
                      h.idx.set, h.idx.segment, ntuple(i->h.idx.user[i],8))

  head = AcquisitionHeader( h.version, h.flags, h.measurement_uid,
    h.scan_counter, h.acquisition_time_stamp,
    ntuple(i->h.physiology_time_stamp[i],3),
    h.number_of_samples, h.available_channels, h.active_channels,
    ntuple(i->h.channel_mask[i],16),
    h.discard_pre, h.discard_post,
    h.center_sample, h.encoding_space_ref, h.trajectory_dimensions, h.sample_time_us,
    ntuple(i->h.position[i],3),
    ntuple(i->h.read_dir[i],3),
    ntuple(i->h.phase_dir[i],3),
    ntuple(i->h.slice_dir[i],3),
    ntuple(i->h.patient_table_position[i],3),
    idx,
    ntuple(i->h.user_int[i],8),
    ntuple(i->h.user_float[i],8)
    )

  return head
end

function FileIO.save(f::ISMRMRDFile, acq::RawAcquisitionData, dataset="dataset")
  h5open(f.filename, "w") do file
    headerStr = GeneralParametersToXML(acq.params)
    write(file, "/$(dataset)/xml", [headerStr]) # [] ensures that we store as var string
    writeProfiles(file, "/$(dataset)/data", acq.profiles)
  end
end


text:
include("partitioning.jl")

struct NewtonSolver
    buildKKT::Function
    cost::Function
    cI::Function
    cE::Function
    d::Function
    active_set::Function
    build_results::Function
end

function get_conditioned_newton_solver(solver::Solver)
    n,m,N = get_sizes(solver)
    p,pI,pE = get_num_constraints(solver)
    p_N,pI_N,pE_N = get_num_terminal_constraints(solver)

    c_function!, c_jacobian!, c_labels, cI!, cE! = generate_constraint_functions(solver.obj)
    f = solver.fd

    Nz,Ng,Nh,Nx = get_batch_sizes2(solver)
    NN = Nz+Ng+Ng+Nh+Nx

    names = (:z,:s,:μ,:λ,:ν)
    ind1 = create_partition([Nz,Ng,Ng,Nh,Nx],names)
    ind2 = create_partition2([Nz,Ng,Ng,Nh,Nx],names)
    indx = 1:N*n
    indu = N*n .+ (1:(N-1)*m)
    s_x = reshape(1:(N*n),n,N)
    s_u = reshape(N*n .+ (1:(m*(N-1))),m,N-1)

    function objective_cost(Z::Vector)
        X,U = unpackZZ(Z)
        J = cost(solver,X,U)
    end

    function al_cost(V::Vector,ρ=1000)
        X,U,s,μ,λ,ν = unpackV(V)
        e = cE(V)
        i = cI(V) + 0.5s.^2
        dd = d(V)
        J = cost(solver,X,U) + λ'e + μ'i + ν'dd + 0.5*ρ*(e'e + i'i + dd'dd)
        return J
    end

    function cI(Z)
        X,U = unpackZZ(Z)
        C = zeros(eltype(Z),pI,N-1)
        for k = 1:N-1
            cI!(view(C,1:pI,k),X[:,k],U[:,k])
        end
        CN = zeros(eltype(Z),pI_N)
        cI!(CN,X[:,N])
        CI = [vec(C); CN]
        return CI
    end

    function cE(Z)
        X,U = unpackZZ(Z)
        C = zeros(eltype(Z),pE,N-1)
        for k = 1:N-1
            cE!(view(C,1:pE,k),X[:,k],U[:,k])
        end
        CN = zeros(eltype(Z),pE_N)
        cE!(CN,X[:,N])
        CE = [vec(C); CN]
        return CE
    end

    function d(Z)
        X,U = unpackZZ(Z)
        D = zeros(eltype(Z),n,N)
        D[:,1] = X[:,1] - solver.obj.x0
        for k = 2:N
            f(view(D,1:n,k),X[:,k-1],U[:,k-1])
            D[:,k] -= X[:,k]
        end
        return vec(D)
    end

    function calculate_jacobians(Z::Vector)
        ∇²J = ForwardDiff.hessian(objective_cost,Z)
        ∇J = ForwardDiff.gradient(objective_cost,Z)
        G = ForwardDiff.jacobian(cI,Z)
        H = ForwardDiff.jacobian(cE,Z)
        D = ForwardDiff.jacobian(d,Z)
        return ∇²J,∇J, G,H,D
    end

    function active_set(Z,eps=1e-2)
        X,U = unpackZZ(Z)
        a = ones(Bool,NN)
        ci = cI(Z)
        c_inds = ones(Bool,length(ci))
        c_inds = ci .>= -eps

        a[ind1.μ] = c_inds
        a[ind1.s] = c_inds
        return a
    end

    function packV(X,U,s,μ,λ,ν)
        V = [vec(X);vec(U);s;μ;λ;ν]
    end

    function unpackV(V)
        X = V[s_x]
        U = V[s_u]
        s = V[ind1.s]
        μ = V[ind1.μ]
        λ = V[ind1.λ]
        ν = V[ind1.ν]
        return X,U,s,μ,λ,ν
    end

    function packZZ(X,U)
        Z = [vec(X);vec(U)]
    end

    function unpackZZ(Z)
        X = Z[s_x]
        U = Z[s_u]
        return X,U
    end

    function buildKKT(V,ρ=1000,type=:regularized,reg=0)
        X,U,s,μ,λ,ν = unpackV(V)
        Z = packZZ(X,U)
        ∇²J,∇J,G,H,D = calculate_jacobians(Z)
        g = cI(Z)
        h = cE(Z)
        dd = d(Z)

        A = spzeros(NN,NN)
        b = zeros(NN)

        if type == :regularized
            A[ind2.zz] = ∇²J
            A[ind2.zμ] = G'
            A[ind2.zλ] = H'
            A[ind2.zν] = D'

            A[ind2.μz] = G
            A[ind2.λz] = H
            A[ind2.νz] = D

            A[ind2.ss] = Diagonal(μ)
            A[ind2.sμ] = Diagonal(s)
            A[ind2.μs] = Diagonal(s)

            A[ind2.μμ] = -1/ρ*Diagonal(I,Ng)
            A[ind2.λλ] = -1/ρ*Diagonal(I,Nh)
            A[ind2.νν] = -1/ρ*Diagonal(I,Nx)

            b[ind1.z] = ∇J + G'μ + H'λ + D'ν
            b[ind1.s] = Diagonal(s)*μ
            b[ind1.μ] = g+s.^2/2
            b[ind1.λ] = h
            b[ind1.ν] = dd
        else
            A[ind2.zz] = ∇²J + ρ*(G'G + H'H + D'D)
            A[ind2.zs] = ρ*G'Diagonal(s)
            A[ind2.zμ] = G'
            A[ind2.zλ] = H'
            A[ind2.zν] = D'

            A[ind2.sz] = ρ*Diagonal(s)*G
            A[ind2.μz] = G
            A[ind2.λz] = H
            A[ind2.νz] = D

            A[ind2.ss] = Diagonal(μ) + ρ*Diagonal(g) + 3/2*Diagonal(s.^2)
            A[ind2.sμ] = Diagonal(s)
            A[ind2.μs] = Diagonal(s)

            A[ind2.μμ] = -reg*Diagonal(I,Ng)
            A[ind2.λλ] = -reg*Diagonal(I,Nh)
            A[ind2.νν] = -reg*Diagonal(I,Nx)

            b[ind1.z] = ∇J + G'μ + H'λ + D'ν + ρ*(H'h + G'*(g+0.5s.^2) + D'dd)
            b[ind1.s] = Diagonal(s)*μ + μ.*s + ρ*g.*s + ρ/2*s.^3
            b[ind1.μ] = g+s.^2/2
            b[ind1.λ] = h
            b[ind1.ν] = dd
        end

        return A,b
    end

    function getV(results::SolverIterResults)
        X = to_array(results.X)
        U = to_array(results.U)

        ν = zeros(n,N-1)
        s = zeros(pI,N-1)
        μ = zeros(pI,N-1)
        λ = zeros(pE,N-1)

        ineq = 1:pI
        eq = pI .+ (1:pE)

        if results isa ConstrainedIterResults
            for k = 1:N-1
                s[:,k] = sqrt.(2.0*max.(0,-results.C[k][ineq]))
                μ[:,k] = results.λ[k][ineq]
                λ[:,k] = results.λ[k][eq]
            end
            append!(s, sqrt.(2.0*max.(0,-results.C[N][1:pI_N])))
            append!(μ, results.λ[N][1:pI_N])
            append!(λ, results.λ[N][pI_N .+ (1:pE_N)])
        end
        for k = 1:N-1
            ν[:,k] = results.s[k]
        end
        s = vec(s)
        μ = vec(μ)
        λ = vec(λ)
        ν = vec(ν)
        append!(ν, results.s[N])

        V = packV(X,U,s,μ,λ,ν)
    end

    # return buildKKT, getV, packV, unpackV, packZZ, unpackZZ, al_cost, cI, cE, d, active_set, calculate_jacobians, ind1, ind2
    NewtonSolver(buildKKT,al_cost,cI,cE,d,active_set,getV)
end

function get_batch_sizes2(solver::Solver)
    n,m,N = get_sizes(solver)
    p,pI,pE = get_num_constraints(solver)
    p_N,pI_N,pE_N = get_num_terminal_constraints(solver)

    Nz = n*N + m*(N-1)
    Ng = pI*(N-1) + pI_N
    Nh = pE*(N-1) + pE_N
    Nx = N*n
    return Nz,Ng,Nh,Nx
end

function armijo_line_search(merit::Function,V,d,grad; max_iter=10, ϕ=0.01)
    α = 1
    J_prev = merit(V)
    J = Inf
    iter = 1
    while J > J_prev + α*ϕ*grad'd
        α *= 0.75
        J = merit(V - α*d)
        if iter > max_iter
            println("Max iterations")
            break
        end
        iter += 1
    end
    return α
end

model,obj = Dynamics.dubinscar
# obj_c = ConstrainedObjective(obj,u_min=-0.3,u_max=0.4)
solver = Solver(model,obj,N=21)
n,m,N = get_sizes(solver)
U0 = ones(m,N-1)
solver.opts.verbose = true
res,stats = solve(solver,U0)
cost(solver,res)
max_violation(res)
λ_update_default!(res,solver)

n_solver = get_conditioned_newton_solver(solver)
cI = n_solver.cI; cE = n_solver.cE; d = n_solver.d
max_c(V) = max(maximum(cI(V)),norm(cE(V),Inf),norm(d(V),Inf))

ρ = 1e6
merit(V) = n_solver.cost(V,ρ)
V = n_solver.build_results(res)
J0 = merit(V)
c_max0 = max_c(V)

J_prev = merit(V)
A,b = n_solver.buildKKT(V,ρ)
a = n_solver.active_set(V,1)
rank(Array(A))
cond(Array(A))

reg = 1
δV = zero(V)
δV[a] = A[a,a]\b[a]
δV = (A+reg*I)\b
α = armijo_line_search(merit,V,δV,b)
V1 = V - δV*α
merit(V1)
@show J_prev - merit(V1)
@show max_c(V) / max_c(V1)
max_c(V1)
V = copy(V1)

# Dubinscar


text:
################################################################################
#                                 Euler Angles
################################################################################

export angle2dcm,      angle2quat,      angle2rot
export smallangle2dcm, smallangle2quat, smallangle2rot

################################################################################
#                                 Conversions
################################################################################

# Direction Cosine Matrix
# ==============================================================================

"""
    function angle2dcm(angle_r1::Number, angle_r2::Number, angle_r3::Number, rot_seq::Symbol = :ZYX)

Convert the Euler angles `angle_r1`, `angle_r2`, and `angle_r3` [rad] with the
rotation sequence `rot_seq` to a direction cosine matrix.

The rotation sequence is defined by a `:Symbol`. The possible values are:
`:XYX`, `XYZ`, `:XZX`, `:XZY`, `:YXY`, `:YXZ`, `:YZX`, `:YZY`, `:ZXY`, `:ZXZ`,
`:ZYX`, and `:ZYZ`. If no value is specified, then it defaults to `:ZYX`.

# Remarks

This function assigns `dcm = A3 * A2 * A1` in which `Ai` is the DCM related with
the *i*-th rotation, `i Є [1,2,3]`.

# Example

```julia-repl
dcm = angle2dcm(pi/2, pi/3, pi/4, :ZYX)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  3.06162e-17  0.5       -0.866025
 -0.707107     0.612372   0.353553
  0.707107     0.612372   0.353553
```

"""
function angle2dcm(angle_r1::Number,
                   angle_r2::Number,
                   angle_r3::Number,
                   rot_seq::Symbol = :ZYX)
    # Compute the sines and cosines.
    c1 = cos(angle_r1)
    s1 = sin(angle_r1)

    c2 = cos(angle_r2)
    s2 = sin(angle_r2)

    c3 = cos(angle_r3)
    s3 = sin(angle_r3)

    if rot_seq == :ZYX
        return DCM(       c2*c1    ,        c2*s1    , -s2  ,
                   s3*s2*c1 - c3*s1, s3*s2*s1 + c3*c1, s3*c2,
                   c3*s2*c1 + s3*s1, c3*s2*s1 - s3*c1, c3*c2)'

    elseif rot_seq == :XYX
        return DCM( c2  ,         s1*s2    ,       -c1*s2    ,
                   s2*s3, -s1*c2*s3 + c1*c3, c1*c2*s3 + s1*c3,
                   s2*c3, -s1*c3*c2 - c1*s3, c1*c3*c2 - s1*s3)'

    elseif rot_seq == :XYZ
        return DCM( c2*c3,  s1*s2*c3 + c1*s3, -c1*s2*c3 + s1*s3,
                   -c2*s3, -s1*s2*s3 + c1*c3,  c1*s2*s3 + s1*c3,
                      s2 ,        -s1*c2    ,       c1*c2)'

    elseif rot_seq == :XZX
        return DCM(   c2 ,         c1*s2    ,         s1*s2    ,
                   -s2*c3,  c1*c3*c2 - s1*s3,  s1*c3*c2 + c1*s3,
                    s2*s3, -c1*c2*s3 - s1*c3, -s1*c2*s3 + c1*c3)'

    elseif rot_seq == :XZY
        return DCM(c3*c2, c1*c3*s2 + s1*s3, s1*c3*s2 - c1*s3,
                    -s2 ,        c1*c2    ,        s1*c2    ,
                   s3*c2, c1*s2*s3 - s1*c3, s1*s2*s3 + c1*c3)'

    elseif rot_seq == :YXY
        return DCM(-s1*c2*s3 + c1*c3, s2*s3 , -c1*c2*s3 - s1*c3,
                          s1*s2    ,   c2  ,         c1*s2    ,
                   s1*c3*c2 + c1*s3, -s2*c3,  c1*c3*c2 - s1*s3)'

    elseif rot_seq == :YXZ
        return DCM( c1*c3 + s2*s1*s3, c2*s3, -s1*c3 + s2*c1*s3,
                   -c1*s3 + s2*s1*c3, c2*c3,  s1*s3 + s2*c1*c3,
                        s1*c2       ,  -s2 ,      c2*c1       )'

    elseif rot_seq == :YZX
        return DCM(        c1*c2    ,    s2 ,        -s1*c2    ,
                   -c3*c1*s2 + s3*s1,  c2*c3,  c3*s1*s2 + s3*c1,
                    s3*c1*s2 + c3*s1, -s3*c2, -s3*s1*s2 + c3*c1)'

    elseif rot_seq == :YZY
        return DCM(c1*c3*c2 - s1*s3, s2*c3, -s1*c3*c2 - c1*s3,
                         -c1*s2    ,   c2 ,         s1*s2    ,
                   c1*c2*s3 + s1*c3, s2*s3, -s1*c2*s3 + c1*c3)'

    elseif rot_seq == :ZXY
        return DCM(c3*c1 - s2*s3*s1, c3*s1 + s2*s3*c1, -s3*c2,
                      -c2*s1       ,     c2*c1       ,    s2 ,
                   s3*c1 + s2*c3*s1, s3*s1 - s2*c3*c1,  c2*c3)'

    elseif rot_seq == :ZXZ
        return DCM(-s1*c2*s3 + c1*c3, c1*c2*s3 + s1*c3, s2*s3,
                   -s1*c3*c2 - c1*s3, c1*c3*c2 - s1*s3, s2*c3,
                           s1*s2    ,       -c1*s2    ,  c2)'

    elseif rot_seq == :ZYZ
        return DCM( c1*c3*c2 - s1*s3,  s1*c3*c2 + c1*s3, -s2*c3,
                   -c1*c2*s3 - s1*c3, -s1*c2*s3 + c1*c3,  s2*s3,
                           c1*s2    ,         s1*s2    ,    c2)'
    else
        throw(ArgumentError("The rotation sequence :$rot_seq is not valid."))
    end
end

"""
    function angle2dcm(eulerang::EulerAngles)

Convert the Euler angles `eulerang` (see `EulerAngles`) to a direction cosine
matrix.

# Returns

The direction cosine matrix.

# Remarks

This function assigns `dcm = A3 * A2 * A1` in which `Ai` is the DCM related with
the *i*-th rotation, `i Є [1,2,3]`.

# Example

```julia-repl
julia> angle2dcm(EulerAngles(pi/2, pi/3, pi/4, :ZYX))
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  3.06162e-17  0.5       -0.866025
 -0.707107     0.612372   0.353553
  0.707107     0.612372   0.353553
```

"""
angle2dcm(eulerang::EulerAngles) = angle2dcm(eulerang.a1,
                                             eulerang.a2,
                                             eulerang.a3,
                                             eulerang.rot_seq)

"""
    function smallangle2dcm(θx::Number, θy::Number, θz::Number)

Create a direction cosine matrix from three small rotations of angles `θx`,
`θy`, and `θz` [rad] about the axes X, Y, and Z, respectively.

# Remarks

No process of ortho-normalization is performed with the computed DCM.

# Example

```julia-repl
julia> smallangle2dcm(+0.01, -0.01, +0.01)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  1.0    0.01  0.01
 -0.01   1.0   0.01
 -0.01  -0.01  1.0
```

"""
smallangle2dcm(θx::Number, θy::Number, θz::Number) = DCM(  1, +θz, -θy,
                                                         -θz,   1, +θx,
                                                         +θy, -θx,   1)'

# Quaternion
# ==============================================================================

"""
    function angle2quat(angle_r1::Number, angle_r2::Number, angle_r3::Number, rot_seq::AbstractString="ZYX")

Convert the Euler angles `angle_r1`, `angle_r2`, and `angle_r3` [rad] with the
rotation sequence `rot_seq` to a quaternion.

The rotation sequence is defined by a `:Symbol`. The possible values are:
`:XYX`, `XYZ`, `:XZX`, `:XZY`, `:YXY`, `:YXZ`, `:YZX`, `:YZY`, `:ZXY`, `:ZXZ`,
`:ZYX`, and `:ZYZ`. If no value is specified, then it defaults to `:ZYX`.

# Remarks

This function assigns `q = q1 * q2 * q3` in which `qi` is the quaternion related
with the *i*-th rotation, `i Є [1,2,3]`.

# Example

```julia-repl
julia> angle2quat(pi/2, pi/3, pi/4, :ZYX)
Quaternion{Float64}:
  + 0.7010573846499779 - 0.09229595564125714.i + 0.5609855267969309.j + 0.43045933457687935.k
```

"""
function angle2quat(angle_r1::Number,
                    angle_r2::Number,
                    angle_r3::Number,
                    rot_seq::Symbol = :ZYX)

    # Compute the sines and cosines of half angle.
    c1 = cos(angle_r1/2)
    s1 = sin(angle_r1/2)

    c2 = cos(angle_r2/2)
    s2 = sin(angle_r2/2)

    c3 = cos(angle_r3/2)
    s3 = sin(angle_r3/2)

    if rot_seq == :ZYX
        q0 = c1*c2*c3 + s1*s2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*c2*s3 - s1*s2*c3),
                          s*(c1*s2*c3 + s1*c2*s3),
                          s*(s1*c2*c3 - c1*s2*s3))
    elseif rot_seq == :XYX
        q0 = c1*c2*c3 - s1*c2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*c2*s3 + s1*c2*c3),
                          s*(c1*s2*c3 + s1*s2*s3),
                          s*(s1*s2*c3 - c1*s2*s3))
    elseif rot_seq == :XYZ
        q0 = c1*c2*c3 - s1*s2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(s1*c2*c3 + c1*s2*s3),
                          s*(c1*s2*c3 - s1*c2*s3),
                          s*(c1*c2*s3 + s1*s2*c3))
    elseif rot_seq == :XZX
        q0 = c1*c2*c3 - s1*c2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*c2*s3 + s1*c2*c3),
                          s*(c1*s2*s3 - s1*s2*c3),
                          s*(c1*s2*c3 + s1*s2*s3))
    elseif rot_seq == :XZY
        q0 = c1*c2*c3 + s1*s2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(s1*c2*c3 - c1*s2*s3),
                          s*(c1*c2*s3 - s1*s2*c3),
                          s*(c1*s2*c3 + s1*c2*s3))
    elseif rot_seq == :YXY
        q0 = c1*c2*c3 - s1*c2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*s2*c3 + s1*s2*s3),
                          s*(c1*c2*s3 + s1*c2*c3),
                          s*(c1*s2*s3 - s1*s2*c3))
    elseif rot_seq == :YXZ
        q0 = c1*c2*c3 + s1*s2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*s2*c3 + s1*c2*s3),
                          s*(s1*c2*c3 - c1*s2*s3),
                          s*(c1*c2*s3 - s1*s2*c3))
    elseif rot_seq == :YZX
        q0 = c1*c2*c3 - s1*s2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*c2*s3 + s1*s2*c3),
                          s*(s1*c2*c3 + c1*s2*s3),
                          s*(c1*s2*c3 - s1*c2*s3))
    elseif rot_seq == :YZY
        q0 = c1*c2*c3 - s1*c2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(s1*s2*c3 - c1*s2*s3),
                          s*(c1*c2*s3 + s1*c2*c3),
                          s*(c1*s2*c3 + s1*s2*s3))
    elseif rot_seq == :ZXY
        q0 = c1*c2*c3 - s1*s2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*s2*c3 - s1*c2*s3),
                          s*(c1*c2*s3 + s1*s2*c3),
                          s*(s1*c2*c3 + c1*s2*s3))
    elseif rot_seq == :ZXZ
        q0 = c1*c2*c3 - s1*c2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*s2*c3 + s1*s2*s3),
                          s*(s1*s2*c3 - c1*s2*s3),
                          s*(c1*c2*s3 + s1*c2*c3))
    elseif rot_seq == :ZYZ
        q0 = c1*c2*c3 - s1*c2*s3

        s = (q0 < 0) ? -1 : +1

        return Quaternion(s*q0,
                          s*(c1*s2*s3 - s1*s2*c3),
                          s*(c1*s2*c3 + s1*s2*s3),
                          s*(c1*c2*s3 + s1*c2*c3))
    else
        throw(ArgumentError("The rotation sequence :$rot_seq is not valid."))
    end
end

"""
    function angle2quat(eulerang::EulerAngles)

Convert the Euler angles `eulerang` (see `EulerAngles`) to a quaternion.

# Remarks

This function assigns `q = q1 * q2 * q3` in which `qi` is the quaternion related
with the *i*-th rotation, `i Є [1,2,3]`.

# Example

```julia-repl
julia> angle2quat(pi/2, pi/3, pi/4, :ZYX)
Quaternion{Float64}:
  + 0.7010573846499779 - 0.09229595564125714.i + 0.5609855267969309.j + 0.43045933457687935.k
```

"""
angle2quat(eulerang::EulerAngles) = angle2quat(eulerang.a1,
                                               eulerang.a2,
                                               eulerang.a3,
                                               eulerang.rot_seq)

"""
    function smallangle2quat(θx::Number, θy::Number, θz::Number)

Create a quaternion from three small rotations of angles `θx`, `θy`, and `θz`
[rad] about the axes X, Y, and Z, respectively.

# Remarks

The quaternion is normalized.

# Example

```julia-repl
julia> smallangle2quat(+0.01, -0.01, +0.01)
Quaternion{Float64}:
  + 0.9999625021092433 + 0.004999812510546217.i - 0.004999812510546217.j + 0.004999812510546217.k
```

"""
function smallangle2quat(θx::Number, θy::Number, θz::Number)
    q0     = 1
    q1     = θx/2
    q2     = θy/2
    q3     = θz/2
    norm_q = sqrt(q0^2 + q1^2 + q2^2 + q3^2)

    Quaternion(q0/norm_q, q1/norm_q, q2/norm_q, q3/norm_q)
end

################################################################################
#                                     API
################################################################################

"""
    function angle2rot([T,] angle_r1::Number, angle_r2::Number, angle_r3::Number, rot_seq::Symbol = :ZYX)

Convert the Euler angles `angle_r1`, `angle_r2`, and `angle_r3` [rad] with the
rotation sequence `rot_seq` to a rotation description of type `T`, which can be
`DCM` or `Quaternion`. If the type `T` is not specified, then it defaults to
`DCM`.

The rotation sequence is defined by a `:Symbol`. The possible values are:
`:XYX`, `XYZ`, `:XZX`, `:XZY`, `:YXY`, `:YXZ`, `:YZX`, `:YZY`, `:ZXY`, `:ZXZ`,
`:ZYX`, and `:ZYZ`. If no value is specified, then it defaults to `:ZYX`.

# Example

```julia-repl
julia> dcm = angle2rot(pi/2, pi/3, pi/4, :ZYX)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  3.06162e-17  0.5       -0.866025
 -0.707107     0.612372   0.353553
  0.707107     0.612372   0.353553

julia> q   = angle2rot(Quaternion,pi/2, pi/3, pi/4, :ZYX)
Quaternion{Float64}:
  + 0.7010573846499779 - 0.09229595564125714.i + 0.5609855267969309.j + 0.43045933457687935.k
```

"""
@inline angle2rot(θx::Number, θy::Number, θz::Number, rot_seq::Symbol) =
    angle2dcm(θx, θy, θz, rot_seq)

@inline angle2rot(::Type{DCM},
                  θx::Number,
                  θy::Number,
                  θz::Number,
                  rot_seq::Symbol) =
    angle2dcm(θx, θy, θz, rot_seq)

@inline angle2rot(::Type{Quaternion},
                  θx::Number,
                  θy::Number,
                  θz::Number,
                  rot_seq::Symbol) =
    angle2quat(θx, θy, θz, rot_seq)

"""
    function angle2rot([T,] angle_r1::Number, angle_r2::Number, angle_r3::Number, rot_seq::Symbol = :ZYX)

Convert the Euler angles `eulerang` (see `EulerAngles`) to a rotation
description of type `T`, which can be `DCM` or `Quaternion`. If the type `T` is
not specified, then it defaults to `DCM`.

# Example

```julia-repl
julia> dcm = angle2rot(EulerAngles(pi/2, pi/3, pi/4, :ZYX))
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  3.06162e-17  0.5       -0.866025
 -0.707107     0.612372   0.353553
  0.707107     0.612372   0.353553

julia> q   = angle2rot(Quaternion,EulerAngles(pi/2, pi/3, pi/4, :ZYX))
Quaternion{Float64}:
  + 0.7010573846499779 - 0.09229595564125714.i + 0.5609855267969309.j +
  0.43045933457687935.k
```

"""
@inline angle2rot(eulerang::EulerAngles) =
    angle2dcm(eulerang.a1, eulerang.a2, eulerang.a3, eulerang.rot_seq)

@inline angle2rot(::Type{DCM}, eulerang::EulerAngles) =
    angle2dcm(eulerang.a1, eulerang.a2, eulerang.a3, eulerang.rot_seq)

@inline angle2rot(::Type{Quaternion}, eulerang::EulerAngles) =
    angle2quat(eulerang.a1, eulerang.a2, eulerang.a3, eulerang.rot_seq)

"""
    function smallangle2rot([T,] θx::Number, θy::Number, θz::Number)

Create a rotation description of type `T` from three small rotations of angles
`θx`, `θy`, and `θz` [rad] about the axes X, Y, and Z, respectively.

The type `T` of the rotation description can be `DCM` or `Quaternion`. If the
type `T` is not specified, then if defaults to `DCM`.

# Returns

The rotation description according to the type `T`.

# Example

```julia-repl
julia> dcm = smallangle2rot(+0.01, -0.01, +0.01);

julia> q   = smallangle2rot(Quaternion,+0.01, -0.01, +0.01);

julia> dcm = smallangle2rot(+0.01, -0.01, +0.01)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  1.0    0.01  0.01
 -0.01   1.0   0.01
 -0.01  -0.01  1.0

julia> q   = smallangle2rot(Quaternion,+0.01, -0.01, +0.01)
Quaternion{Float64}:
  + 0.9999625021092433 + 0.004999812510546217.i - 0.004999812510546217.j + 0.004999812510546217.k
```

"""
@inline smallangle2rot(θx::Number, θy::Number, θz::Number) =
    smallangle2dcm(θx, θy, θz)

@inline smallangle2rot(::Type{DCM}, θx::Number, θy::Number, θz::Number) =
    smallangle2dcm(θx, θy, θz)

@inline smallangle2rot(::Type{Quaternion}, θx::Number, θy::Number, θz::Number) =
    smallangle2quat(θx, θy, θz)