Calculate the ABCD-linearization of an SLH model

    Return the A, B, C, D and (a, c) matrices that linearize an SLH model
    about a coherent displacement amplitude a0.

    The equations of motion and the input-output relation are then:

    dX = (A X + a) dt + B dA_in
    dA_out = (C X + c) dt + D dA_in

    where, if doubled_up == False

        dX = [a_1, ..., a_m]
        dA_in = [dA_1, ..., dA_n]

    or if doubled_up == True

        dX = [a_1, ..., a_m, a_1^*, ... a_m^*]
        dA_in = [dA_1, ..., dA_n, dA_1^*, ..., dA_n^*]

    Args:
        slh: SLH object
        a0: dictionary of coherent amplitudes ``{a1: a1_0, a2: a2_0, ...}``
            with annihilation mode operators as keys and (numeric or symbolic)
            amplitude as values.
        doubled_up: boolean, necessary for phase-sensitive / active systems


    Returns:

        A tuple (A, B, C, D, a, c])

        with

        * `A`: coupling of modes to each other
        * `B`: coupling of external input fields to modes
        * `C`: coupling of internal modes to output
        * `D`: coupling of external input fields to output fields

        * `a`: constant coherent input vector for mode e.o.m.
        * `c`: constant coherent input vector of scattered amplitudes
            contributing to the output

def getABCD(slh, a0=None, doubled_up=True):
    """Calculate the ABCD-linearization of an SLH model

    Return the A, B, C, D and (a, c) matrices that linearize an SLH model
    about a coherent displacement amplitude a0.

    The equations of motion and the input-output relation are then:

    dX = (A X + a) dt + B dA_in
    dA_out = (C X + c) dt + D dA_in

    where, if doubled_up == False

        dX = [a_1, ..., a_m]
        dA_in = [dA_1, ..., dA_n]

    or if doubled_up == True

        dX = [a_1, ..., a_m, a_1^*, ... a_m^*]
        dA_in = [dA_1, ..., dA_n, dA_1^*, ..., dA_n^*]

    Args:
        slh: SLH object
        a0: dictionary of coherent amplitudes ``{a1: a1_0, a2: a2_0, ...}``
            with annihilation mode operators as keys and (numeric or symbolic)
            amplitude as values.
        doubled_up: boolean, necessary for phase-sensitive / active systems


    Returns:

        A tuple (A, B, C, D, a, c])

        with

        * `A`: coupling of modes to each other
        * `B`: coupling of external input fields to modes
        * `C`: coupling of internal modes to output
        * `D`: coupling of external input fields to output fields

        * `a`: constant coherent input vector for mode e.o.m.
        * `c`: constant coherent input vector of scattered amplitudes
            contributing to the output
    """
    from qnet.algebra.library.fock_operators import Create, Destroy
    if a0 is None:
        a0 = {}

    # the different degrees of freedom
    full_space = ProductSpace.create(slh.S.space, slh.L.space, slh.H.space)
    modes = sorted(full_space.local_factors)

    # various dimensions
    ncav = len(modes)
    cdim = slh.cdim

    # initialize the matrices
    if doubled_up:
        A = np.zeros((2*ncav, 2*ncav), dtype=object)
        B = np.zeros((2*ncav, 2*cdim), dtype=object)
        C = np.zeros((2*cdim, 2*ncav), dtype=object)
        a = np.zeros(2*ncav, dtype=object)
        c = np.zeros(2*cdim, dtype=object)

    else:
        A = np.zeros((ncav, ncav), dtype=object)
        B = np.zeros((ncav, cdim), dtype=object)
        C = np.zeros((cdim, ncav), dtype=object)
        a = np.zeros(ncav, dtype=object)
        c = np.zeros(cdim, dtype=object)

    def _as_complex(o):
        if isinstance(o, Operator):
            o = o.expand()
            if o is IdentityOperator:
                o = 1
            elif o is ZeroOperator:
                o = 0
            elif isinstance(o, ScalarTimesOperator):
                assert o.term is IdentityOperator
                o = o.coeff
            else:
                raise ValueError("{} is not trivial operator".format(o))
        try:
            return complex(o)
        except TypeError:
            return o

    D = np.array([[_as_complex(o) for o in Sjj] for Sjj in slh.S.matrix])

    if doubled_up:
        # need to explicitly compute D^* because numpy object-dtype array's
        # conjugate() method doesn't work
        Dc = np.array([[D[ii, jj].conjugate() for jj in range(cdim)]
                       for ii in range(cdim)])
        D = np.vstack((np.hstack((D, np.zeros((cdim, cdim)))),
                       np.hstack((np.zeros((cdim, cdim)), Dc))))

    # create substitutions to displace the model
    mode_substitutions = {aj: aj + aj_0 * IdentityOperator
                          for aj, aj_0 in a0.items()}
    mode_substitutions.update({
        aj.dag(): aj.dag() + aj_0.conjugate() * IdentityOperator
        for aj, aj_0 in a0.items()
    })
    if len(mode_substitutions):
        slh_displaced = (slh.substitute(mode_substitutions).expand()
                         .simplify_scalar())
    else:
        slh_displaced = slh

    # make symbols for the external field modes
    noises = [OperatorSymbol('b_{}'.format(n), hs="ext_{}".format(n))
              for n in range(cdim)]

    # compute the QSDEs for the internal operators
    eoms = [slh_displaced.symbolic_heisenberg_eom(Destroy(hs=s), noises=noises)
            for s in modes]

    # use the coefficients to generate A, B matrices
    for jj in range(len(modes)):
        coeffsjj = get_coeffs(eoms[jj])
        a[jj] = coeffsjj[IdentityOperator]
        if doubled_up:
            a[jj+ncav] = coeffsjj[IdentityOperator].conjugate()

        for kk, skk in enumerate(modes):
            A[jj, kk] = coeffsjj[Destroy(hs=skk)]
            if doubled_up:
                A[jj+ncav, kk+ncav] = coeffsjj[Destroy(hs=skk)].conjugate()
                A[jj, kk + ncav] = coeffsjj[Create(hs=skk)]
                A[jj+ncav, kk] = coeffsjj[Create(hs=skk)].conjugate()

        for kk, dAkk in enumerate(noises):
            B[jj, kk] = coeffsjj[dAkk]
            if doubled_up:
                B[jj+ncav, kk+cdim] = coeffsjj[dAkk].conjugate()
                B[jj, kk+cdim] = coeffsjj[dAkk.dag()]
                B[jj + ncav, kk] = coeffsjj[dAkk.dag()].conjugate()

    # use the coefficients in the L vector to generate the C, D
    # matrices
    for jj, Ljj in enumerate(slh_displaced.Ls):
        coeffsjj = get_coeffs(Ljj)
        c[jj] = coeffsjj[IdentityOperator]
        if doubled_up:
            c[jj+cdim] = coeffsjj[IdentityOperator].conjugate()

        for kk, skk in enumerate(modes):
            C[jj, kk] = coeffsjj[Destroy(hs=skk)]
            if doubled_up:
                C[jj+cdim, kk+ncav] = coeffsjj[Destroy(hs=skk)].conjugate()
                C[jj, kk+ncav] = coeffsjj[Create(hs=skk)]
                C[jj+cdim, kk] = coeffsjj[Create(hs=skk)].conjugate()

    return map(SympyMatrix, (A, B, C, D, a, c))