def _smepdpsolve_single_trajectory(L, dt, tlist, N_store, N_substeps, rho_t,
                                   c_ops, e_ops, data):
    """ 
    Internal function.
    """
    states_list = []

    rho_t = np.copy(rho_t)

    prng = RandomState() # todo: seed it
    r_jump, r_op = prng.rand(2)

    jump_times = []
    jump_op_idx = []

    for t_idx, t in enumerate(tlist):

        if e_ops:
            for e_idx, e in enumerate(e_ops):
                data.expect[e_idx, t_idx] += expect_rho_vec(e, rho_t)
        else:
            states_list.append(Qobj(vec2mat(rho_t)))

        for j in range(N_substeps):

            if expect_rho_vec(d_op, sigma_t) < r_jump:
                # jump occurs
                p = np.array([rho_expect(c.dag() * c, rho_t) for c in c_ops])
                p = np.cumsum(p / np.sum(p))
                n = np.where(p >= r_op)[0][0]

                # apply jump
                rho_t = c_ops[n] * psi_t * c_ops[n].dag()
                rho_t /= rho_expect(c.dag() * c, rho_t)
                rho_t = np.copy(rho_t)

                # store info about jump
                jump_times.append(tlist[t_idx] + dt * j)
                jump_op_idx.append(n)

                # get new random numbers for next jump
                r_jump, r_op = prng.rand(2)

            # deterministic evolution wihtout correction for norm decay
            dsigma_t = spmv(L.data.data,
                            L.data.indices,
                            L.data.indptr, sigma_t) * dt

            # deterministic evolution with correction for norm decay
            drho_t = spmv(L.data.data,
                          L.data.indices,
                          L.data.indptr, rho_t) * dt

            rho_t += drho_t

            # increment density matrices
            sigma_t += dsigma_t
            rho_t += drho_t

    return states_list, jump_times, jump_op_idx

def _ssesolve_single_trajectory(H, dt, tlist, N_store, N_substeps, psi_t,
                                A_ops, e_ops, data, rhs, d1, d2, d2_len,
                                homogeneous, distribution, args,
                                store_measurement=False, noise=None):
    """
    Internal function. See ssesolve.
    """

    if noise is None:
        if homogeneous:
            if distribution == 'normal':
                dW = np.sqrt(dt) * scipy.randn(len(A_ops), N_store, N_substeps, d2_len)
            else:
                raise TypeError('Unsupported increment distribution for homogeneous process.')
        else:
            if distribution != 'poisson':
                raise TypeError('Unsupported increment distribution for inhomogeneous process.')

            dW = np.zeros((len(A_ops), N_store, N_substeps, d2_len))
    else:
        dW = noise

    states_list = []
    measurements = np.zeros((len(tlist), len(A_ops)), dtype=complex)

    for t_idx, t in enumerate(tlist):

        if e_ops:
            for e_idx, e in enumerate(e_ops):
                s = cy_expect(e.data.data, e.data.indices, e.data.indptr, psi_t)
                data.expect[e_idx, t_idx] += s
                data.ss[e_idx, t_idx] += s ** 2
        else:
            states_list.append(Qobj(psi_t))

        psi_prev = np.copy(psi_t)

        for j in range(N_substeps):

            if noise is None and not homogeneous:
                for a_idx, A in enumerate(A_ops):
                    dw_expect = norm(spmv(A[0].data, A[0].indices, A[0].indptr, psi_t)) ** 2 * dt
                    dW[a_idx, t_idx, j, :] = np.random.poisson(dw_expect, d2_len)

            psi_t = rhs(H.data, psi_t, t + dt * j,
                        A_ops, dt, dW[:, t_idx, j, :], d1, d2, args)

            # optionally renormalize the wave function: TODO: make the 
            # renormalization optional througha configuration parameter in
            # Odeoptions
            psi_t /= norm(psi_t)

        if store_measurement:
            for a_idx, A in enumerate(A_ops):
                phi = spmv(A[0].data, A[0].indices, A[0].indptr, psi_prev)
                measurements[t_idx, a_idx] = (norm(phi) ** 2 * dt * N_substeps
                                              + dW[a_idx, t_idx, :, 0].sum())


    return states_list, dW, measurements

def d2_rho_heterodyne(A, rho_vec):
    """
    todo: cythonize, docstrings
    """
    M = A[0] + A[3]
    e1 = cy_expect_rho_vec(M, rho_vec)
    d1 = spmv(M.data, M.indices, M.indptr, rho_vec) - e1 * rho_vec
    M = A[0] - A[3]
    e1 = cy_expect_rho_vec(M, rho_vec)
    d2 = spmv(M.data, M.indices, M.indptr, rho_vec) - e1 * rho_vec
    return [1.0/np.sqrt(2) * d1, -1.0j/np.sqrt(2) * d2]

def d1_psi_photocurrent(A, psi):
    """
    Todo: cythonize.

    Note: requires poisson increments

    .. math::

        D_1(\psi, t) = - \\frac{1}{2}(C^\dagger C \psi - ||C\psi||^2 \psi)

    """
    return (-0.5 * (spmv(A[3].data, A[3].indices, A[3].indptr, psi)
            -norm(spmv(A[0].data, A[0].indices, A[0].indptr, psi)) ** 2 * psi))

def _rhs_rho_deterministic(L, rho_t, t, dt, args):
    """
    Deterministic contribution to the density matrix change
    """
    drho_t = spmv(L, rho_t) * dt

    return drho_t

def d1_rho_photocurrent(A, rho_vec):
    """
    Todo: cythonize, add (AdA)_L + AdA_R to precomputed operators
    """
    n_sum = A[4] + A[5]
    e1 = cy_expect_rho_vec(n_sum, rho_vec, 0)
    return 0.5 * (e1 * rho_vec - spmv(n_sum, rho_vec))

def d1_rho_photocurrent(A, rho_vec):
    """
    Todo: cythonize, add (AdA)_L + AdA_R to precomputed operators
    """
    n_sum = A[4] + A[5]
    e1 = cy_expect_rho_vec(n_sum, rho_vec)
    return -spmv(n_sum.data, n_sum.indices, n_sum.indptr, rho_vec) + e1 * rho_vec

def _rhs_psi_platen(H, psi_t, t, A_ops, dt, dW, d1, d2, args):
    """
    TODO: support multiple stochastic increments

    .. note::

        Experimental.

    """

    sqrt_dt = np.sqrt(dt)

    dW_len = len(dW[0, :])
    dpsi_t = _rhs_psi_deterministic(H, psi_t, t, dt, args)

    for a_idx, A in enumerate(A_ops):
        # XXX: This needs to be revised now that 
        # dpsi_t is the change for all stochastic collapse operators

        # TODO: needs to be updated to support mutiple Weiner increments
        dpsi_t_H = (-1.0j * dt) * spmv(H.data, H.indices, H.indptr, psi_t)

        psi_t_1 = psi_t + dpsi_t_H + d1(A, psi_t) * dt + d2(A, psi_t)[0] * dW[a_idx,0]
        psi_t_p = psi_t + dpsi_t_H + d1(A, psi_t) * dt + d2(A, psi_t)[0] * sqrt_dt
        psi_t_m = psi_t + dpsi_t_H + d1(A, psi_t) * dt - d2(A, psi_t)[0] * sqrt_dt

        dpsi_t += 0.50 * (d1(A, psi_t_1) + d1(A, psi_t)) * dt + \
            0.25 * (d2(A, psi_t_p)[0] + d2(A, psi_t_m)[0] + 2 * d2(A, psi_t)[0]) * dW[a_idx,0] + \
            0.25 * (d2(A, psi_t_p)[0] - d2(A, psi_t_m)[0]) * (dW[a_idx,0] ** 2 - dt) * sqrt_dt

    return dpsi_t

def d1_psi_homodyne(A, psi):
    """
    OK
    Todo: cythonize

    .. math::

        D_1(\psi, t) = \\frac{1}{2}(\\langle C + C^\\dagger\\rangle\\psi - 
        C^\\dagger C\\psi - \\frac{1}{4}\\langle C + C^\\dagger\\rangle^2\\psi)

    """

    e1 = cy_expect(A[1].data, A[1].indices, A[1].indptr, psi)
    return 0.5 * (e1 * spmv(A[0].data, A[0].indices, A[0].indptr, psi) -
                  spmv(A[3].data, A[3].indices, A[3].indptr, psi) -
                  0.25 * e1 ** 2 * psi)

def _smesolve_single_trajectory(L, dt, tlist, N_store, N_substeps, rho_t,
                                A_ops, e_ops, data, rhs, d1, d2, d2_len):
    """
    Internal function. See smesolve.
    """

    dW = np.sqrt(dt) * scipy.randn(len(A_ops), N_store, N_substeps, d2_len)

    states_list = []

    for t_idx, t in enumerate(tlist):

        if e_ops:
            for e_idx, e in enumerate(e_ops):
                # XXX: need to keep hilbert space structure
                data.expect[e_idx, t_idx] += expect(e, Qobj(vec2mat(rho_t)))
        else:
            states_list.append(Qobj(rho_t))  # dito

        for j in range(N_substeps):

            drho_t = spmv(L.data.data,
                          L.data.indices,
                          L.data.indptr, rho_t) * dt

            for a_idx, A in enumerate(A_ops):
                drho_t += rhs(L.data, rho_t, A,
                              dt, dW[a_idx, t_idx, j, :], d1, d2)

            rho_t += drho_t

    return states_list

def d1_rho_homodyne(A, rho_vec):
    """
    
    D1[a] rho = lindblad_dissipator(a) * rho

    Todo: cythonize
    """
    return spmv(A[7].data, A[7].indices, A[7].indptr, rho_vec)

def d1_psi_heterodyne(A, psi):
    """
    Todo: cythonize

    .. math::

        D_1(\psi, t) = -\\frac{1}{2}(C^\\dagger C - \\langle C^\\dagger \\rangle C + 
                        \\frac{1}{2}\\langle C \\rangle\\langle C^\\dagger \\rangle))\psi

    """
    e_C = cy_expect(A[0].data, A[0].indices, A[0].indptr, psi) # e_C
    B = A[0].T.conj()
    e_Cd = cy_expect(B.data, B.indices, B.indptr, psi) # e_Cd

    return  (-0.5 * spmv(A[3].data, A[3].indices, A[3].indptr, psi) +
             0.5 * e_Cd * spmv(A[0].data, A[0].indices, A[0].indptr, psi) -
             0.25 * e_C * e_Cd * psi)

def d2_rho_homodyne(A, rho):
    """
    not tested
    Todo: cythonize
    """

    e1 = _rho_expect(A[2], rho)
    return [spmv(A[1].data, A[1].indices, A[1].indptr, rho) - e1 * rho]

def _rhs_rho_milstein_homodyne(L, rho_t, t, A_ops, dt, dW, d1, d2, args):
    """
    .. note::

        Experimental.
        Milstein scheme for homodyne detection.
        This implementation works for commuting stochastic jump operators.
        TODO: optimizations: do calculation for n>m only

    """
    A_len = len(A_ops)

    M = np.array([A_ops[n][0] + A_ops[n][3] for n in range(A_len)])
    e1 = np.array([cy_expect_rho_vec(M[n], rho_t, 0) for n in range(A_len)])

    d1_vec = np.sum([spmv(A_ops[n][7], rho_t)
                     for n in range(A_len)], axis=0)

    d2_vec = np.array([spmv(M[n], rho_t)
                       for n in range(A_len)])

    # This calculation is suboptimal. We need only values for m>n in case of
    # commuting jump operators.
    d2_vec2 = np.array([[spmv(M[n], d2_vec[m])
                         for m in range(A_len)] for n in range(A_len)])
    e2 = np.array([[cy_expect_rho_vec(M[n], d2_vec[m], 0)
                    for m in range(A_len)] for n in range(A_len)])

    drho_t = _rhs_rho_deterministic(L, rho_t, t, dt, args)
    drho_t += d1_vec * dt
    drho_t += np.sum([(d2_vec[n] - e1[n] * rho_t) * dW[n, 0]
                      for n in range(A_len)], axis=0)
    drho_t += 0.5 * np.sum([(d2_vec2[n, n] - 2.0 * e1[n] * d2_vec[n] +
                            (-e2[n, n] + 2.0 * e1[n] * e1[n]) * rho_t) * (dW[n, 0] * dW[n, 0] - dt)
                            for n in range(A_len)], axis=0)

    # This calculation is suboptimal. We need only values for m>n in case of
    # commuting jump operators.
    drho_t += 0.5 * np.sum([(d2_vec2[n, m] - e1[m] * d2_vec[n] - e1[n] * d2_vec[m] +
                          (-e2[n, m] + 2.0 * e1[n] * e1[m]) * rho_t) * (dW[n, 0] * dW[m, 0])
                            for (n, m) in np.ndindex(A_len, A_len) if n != m], axis=0)

    return rho_t + drho_t

def d2_psi_heterodyne(A, psi):
    """
    Todo: cythonize

        X = \\frac{1}{2}(C + C^\\dagger)

        Y = \\frac{1}{2}(C - C^\\dagger)

        D_{2,1}(\psi, t) = \\sqrt(1/2) * (C - \\langle X \\rangle) \\psi

        D_{2,2}(\psi, t) = -i\\sqrt(1/2) * (C - \\langle Y \\rangle) \\psi

    """

    X = 0.5 * cy_expect(A[1].data, A[1].indices, A[1].indptr, psi)
    Y = 0.5 * cy_expect(A[2].data, A[2].indices, A[2].indptr, psi)

    d2_1 = np.sqrt(0.5) * (spmv(A[0].data, A[0].indices, A[0].indptr, psi) - X * psi)
    d2_2 = -1.0j * np.sqrt(0.5) * (spmv(A[0].data, A[0].indices, A[0].indptr, psi) - Y * psi)

    return [d2_1, d2_2]

def d2_rho_homodyne(A, rho_vec):
    """

    D2[a] rho = a rho + rho a^\dagger - Tr[a rho + rho a^\dagger]
              = (A_L + Ad_R) rho_vec - E[(A_L + Ad_R) rho_vec]

    Todo: cythonize, add A_L + Ad_R to precomputed operators
    """
    M = A[0] + A[3]

    e1 = cy_expect_rho_vec(M, rho_vec)
    return [spmv(M.data, M.indices, M.indptr, rho_vec) - e1 * rho_vec]

def d2_psi_homodyne(A, psi):
    """
    OK
    Todo: cythonize

    .. math::

        D_2(\psi, t) = (C - \\frac{1}{2}\\langle C + C^\\dagger\\rangle)\\psi

    """

    e1 = cy_expect(A[1].data, A[1].indices, A[1].indptr, psi)
    return [spmv(A[0].data, A[0].indices, A[0].indptr, psi) - 0.5 * e1 * psi]

def sop_H(A, rho_vec):
    """
    Evaluate the superoperator

    H[a] rho = a rho + rho a^\dagger - Tr[a rho + rho a^\dagger] rho
            -> (A_L + Ad_R) rho_vec - E[(A_L + Ad_R) rho_vec] rho_vec

    Todo: cythonize, add A_L + Ad_R to precomputed operators
    """
    M = A[0] + A[3]

    e1 = cy_expect_rho_vec(M, rho_vec, 0)
    return spmv(M, rho_vec) - e1 * rho_vec

def _rhs_rho_milstein_homodyne_single(L, rho_t, t, A_ops, dt, dW, d1, d2, args):
    """
    .. note::

        Experimental.
        Milstein scheme for homodyne detection with single jump operator.

    """

    A = A_ops[0]
    M = A[0] + A[3]
    e1 = cy_expect_rho_vec(M, rho_t, 0)

    d2_vec = spmv(M, rho_t)
    d2_vec2 = spmv(M, d2_vec)
    e2 = cy_expect_rho_vec(M, d2_vec, 0)

    drho_t = _rhs_rho_deterministic(L, rho_t, t, dt, args)
    drho_t += spmv(A[7], rho_t) * dt
    drho_t += (d2_vec - e1 * rho_t) * dW[0, 0]
    drho_t += 0.5 * (d2_vec2 - 2 * e1 * d2_vec + (-e2 + 2 * e1 * e1) * rho_t) * (dW[0, 0] * dW[0, 0] - dt)
    return rho_t + drho_t

def _rhs_rho_euler_homodyne_fast(L, rho_t, t, A, dt, ddW, d1, d2, args):
	"""
	fast Euler-Maruyama for homodyne detection
	"""

	dW = ddW[:,0]

	d_vec = spmv(A[0][0], rho_t).reshape(-1, len(rho_t))
	e = np.real(d_vec[:-1].reshape(-1,A[0][1],A[0][1]).trace(axis1=1,axis2=2))

	drho_t = d_vec[-1]
	drho_t += np.dot(dW, d_vec[:-1])
	drho_t += (1.0 - np.inner(e, dW)) * rho_t
	return drho_t