/*****************************************************************************
*                                                                            *
*   TF2SS.SPL    Copyright (C) 1998 DSP Development Corporation              *
*                               All Rights Reserved                          *
*                                                                            *
*   Author:      Matthew Tom                                                 *
*                                                                            *
*   Synopsis:    Transfer function to state space conversion                 *
*                                                                            *
*   Revisions:    3 Jul 1997  MAT  Creation                                  *
*                 8 Jun 1998  MAT  Help Menu Entry                           *
*                                                                            *
*****************************************************************************/

#include 

#if @HELP_TF2SS

    TF2SS

    Purpose: Calculates the state-space representation:

                        .
                        x = Ax + Bu
                        y = Cx + Du

             of the system:

                        NUM(s)
                H(s) = --------
                        DEN(s)

         from a single input.

    Syntax:  (A,B,C,D) = tf2ss(Num,Den)

              Num - Matrix NUM must contain the numerator
                    coefficients with as many rows as there are
                    outputs y.

              Den - Vector DEN must contain the coefficients of
                    the denominator in descending powers of s.

    Returns: The A,B,C,D matrices of the state space
             transformation in controller canonical form.

    Example:
             (A,B,C,D) = tf2ss(Num,Den)

             creates the A,B,C,D matrices of the state space
             transformation in controller canonical form.

    Remarks:
             TF2SS also works for discrete systems.

             For discrete systems, use a numerator polynomial padded
             with zeros to the same length as the denominator.

    See Also:
             Bilinear

#endif


/* convert state space */
tf2ss(num, den)
{
        local f1, f2, nnr, nnc, dnr, dnc, nz, a, b, c, d, d1;

        f1 = 0;
        f2 = 0;

        (nnr, nnc) = size(num);
        (dnr, dnc) = size(den);

        if (nnc == 1)
        {
                num = num';
                f1  = 1;
        }

        if (dnc == 1)
        {
                den = den';
                f2  = 1;
        }

        (nnr, nnc) = size(num);
        (dnr, dnc) = size(den);

        /* empty case */
        if (dnc == 0 && nnc == 0)
        {
                return({}, {}, {}, {});
        }

        /* strip leading zeros from denominator */
        nz  = find(not(den == 0));
        den = den[1..dnr, nz[1]..dnc];

        (dnr, dnc) = size(den);

        /* check numerator */
        if (nnc > dnc)
        {
                if (all(all(num[1..nnr, 1..nnc-dnc])) == 0)
                {
                        num = num[1..nnr, (nnc-dnc+1)..nnc];
                        (nnr, nnc) = size(num);
                }
                else
                {
                        error("tf2ss - denominator order must be >= numerator order");
                }
        }

        /* normalization factor */
        d1 = den[1];

        /* zero prepad num to have the same number of columns as den */
        num = ravel(zeros(nnr, dnc - nnc), num) / d1;

        d = (length(num) > 0) ? num[1..nnr, 1] : {};

        /* constant denominator */
        if (dnc == 1)
        {
                return({}, {}, {}, d);
        }

        /* normalize */
        den = den[1..dnr, 2..dnc] / d1;

        /* output arrays */
        a = {-den, eye(dnc - 2, dnc - 1)};
        b = eye(dnc - 1, 1);
        c = (nnr > 0) ? num[1..nnr, 2..dnc] - num[1..nnr, 1] *^ den : {};

        if (f1 == 1)
        {
                num = num';
        }

        if (f2 == 1)
        {
                den = den';
        }

        return(a, b, c, d);
}