/*****************************************************************/
/*                  kihon.mac                                    */
/* (c) Kenrou Adachi, 2005/10/29, 2007/7/1, 2008/6/5, 2021/11/23 */
/* 行列の基本変形パッケージ                                      */
/* Maxima-5.9.39 以降のバージョンを対象とする。(無保証)          */
/*****************************************************************/

/*** for primary education on linear algebra  ***/

/* generating a column vector from a list  */

colvector(A) := transpose(matrix(A))$

/* generating a row vector from a list */

rowvector(A) := matrix(A)$



/* コピーをつくって変形 */

/*** matrix transformations ***/


/* k times of the A's i-th row */

cp_col_sp(A, k, i) := block([B],
        mode_declare(B, any),
        B : copymatrix(A),
        B[i] : k * B[i],
        B
)$


/* adds  k times of the A's j-th row to the A's i-th row  */

cp_col_ad(A, k, i, j) := block([B],
        mode_declare(B, any),
        B : copymatrix(A),
        B[i] : B[i] + k * B[j],
        B
)$


/* exchanges the A's i-th row and the A's j-th row  */

cp_col_ex(A, i, j) := block([B, dummy],
        mode_declare(B, any, dummy, any),
        B : copymatrix(A),
        dummy : B[i],
        B[i] : B[j],
        B[j] : dummy,
        B
)$


/* k times the A's i-th column */

cp_row_sp(A, k, i) := block([B],
        mode_declare(B, matrix),
        B : cp_col_sp(transpose(A), k, i),
        transpose(B)
)$


/* adds k times of the A's j-th column to the A's i-th column */ 

cp_row_ad(A, k, i, j) := block([B],
        mode_declare(B, any),
        B : cp_col_ad(transpose(A), k, i, j),
        transpose(B)
)$


/* exchanges the A's i-th column and the A's j-th column */

cp_row_ex(A, i, j) := block([B],
        mode_declare(B, any),
        B : cp_col_ex(transpose(A), i, j),
        transpose(B)
)$


/* 破壊的変形 */

/** destructive transformation **/

col_sp(A, k, i) := block(
        A[i] : k * A[i],
        A
)$


col_ad(A, k, i, j) := block(
        A[i] : A[i] + k * A[j],
        A
)$


col_ex(A, i, j) := block([dummy],
        dummy : A[i],
        A[i] : A[j],
        A[j] : dummy,
        A
)$


row_sp(A, k, i) := block([len],
        mode_declare(len, integer),
        len : length(transpose(A)[1]),
        for p : 1 thru len do
                A[p][i] : k * A[p][i],
        A
)$


row_ad(A, k, i, j) := block([len],
        mode_declare(len, integer),
        len : length(transpose(A)[1]),
        for p : 1 thru len do
                A[p][i] : A[p][i] + k * A[p][j],
        A
)$


row_ex(A, i, j) := block([len, c],
        mode_declare(len, integer, c, any),
        len : length(transpose(A)[1]),
        for p : 1 thru len do (
                c : A[p][i],
                A[p][i] : A[p][j],
                A[p][j] : c
        ),
        A
)$



/* 途中の変形を表示して階段行列に変形 */
/* あまり賢くないが教育的なアルゴリズムによる */

/* 破壊的変形 */

/* transforms to a matrix of echelon form */


kaidan_findnonzero(A, pi, pj, mm) := block([pix],
    mode_declare(pix, integer),
    pix : pi,
    jump,
    if not A[pix, pj] = 0 then
        pix
    else if pix = mm then
        (-1)
    else
    (
       pix : pix + 1,
       go(jump)
    )
)$


kaidan(A) := block([m, n, i, j, ir],
    mode_declare(m, integer, n, integer, i, integer, j, integer, ir, integer),

    if matrixp(A) = false then
    (
        print(A," is not a matrix"),
        []
    ),

    m : length(transpose(A)[1]),
    n : length(A[1]),
    i : 1,
    j : 1,
    
    loop,
    
    ir : kaidan_findnonzero(A, i, j, m),
    if ir = -1 then
    (
        if j = n then
            go(finish)
        else
        (
            j : j + 1,
            go(loop)
        )
    )
    else if ir > i then
    (
        col_ex(A, i, ir),
        print(A)
    ),

    if not A[i, j] = 1 then
    (
        col_sp(A, 1 / A[i, j], i),
        print(A)
    ),

    if i > 1 then
        for k : 1 thru i - 1 do
            if not A[k, j] = 0 then
            (
                col_ad(A, -A[k, j], k, i),
                print(A)
            ),

    if i < m then
        for k : i + 1 thru m do
            if not A[k, j] = 0 then
            (
                col_ad(A, -A[k, j], k, i),
                print(A)
            ),

    if i < m and j < n then
    (
       i : i + 1,
       j : j + 1,
       go(loop)
    ),
    
    finish,

    A
)$



/* コピーを作って変形 */

cp_kaidan(A) := block([CPA],
  mode_declare(CPA, any),
  CPA : copymatrix(A),
  kaidan(CPA) 
)$




/* 階段行列を標準形に */
/* A が階段行列でなければ意味がない */

/* from an echelon-matrix to a canoncal-matrix  */
/* A must be type of echelon */


kai2hyo_findfirst(A, ii, nn) := block([ij, l],
    mode_declare(ij, integer, l, integer),
    for l : ii thru nn do
        if (A[ii, l] = 1) then
            ij : l
        else
            ij : l + 1,

    if ij <= nn then
        return(ij)
     else
        return(-1)
)$


kai2hyo(A) := block([m, n, minima],
    mode_declare(m, integer, n, integer, minima, integer),
    
    if matrixp(A) = false then
    (
        print(A, " is not a matrix!"),
        []
    ),

    n : length(A[1]),
    m : length(transpose(A)[1]),

    if m > n then
        minima : n
    else
        minima : m,
    
    block([j],
        mode_declare(j, integer),
        for i : 1 thru minima do
        (
            j : kai2hyo_findfirst(A, i, n),
            if (not j = -1) and (not i = j) then
            (
                row_ex(A, i, j),
                print(A)
            ),
            if i < n then
                for k : i + 1 thru n do
                    if not A[i, k] = 0 then
                    (
                        row_ad(A, -A[i, k], k, i),
                        print(A)
                    )
        )
    ),
    A
)$



/* 階段行列をコピーをつくってから標準形に */

cp_kai2hyo(A) := block([CPA],
    mode_declare(CPA, integer),
    CPA : copymatrix(A),
    kai2hyo(CPA)
)$



/* 行列を標準形に */

hyoujyun(A) := kai2hyo(kaidan(A))$



/* コピーをつくって標準形に */

cp_hyoujyun(A) := kai2hyo(cp_kaidan(A))$



/* 行列の切りだし */
/* an utility */

matblock(Mat, i1, i2, j1, j2) := block([A, m, n],
    mode_declare(A, any, m, integer, n, integer),
    m : i2 - i1,  n : j2 - j1,
    A : zeromatrix(m + 1, n + 1),
    for i : 1 thru m + 1 do
        for j : 1 thru n + 1 do
            A[i][j] : Mat[i1 + i - 1][j1 + j - 1],
    A
)$



/* 正則行列 A の逆行列を計算過程を表示して求める。 */

/* inverse matrix */

myinv(A) := block([I, AI, P, m, n],
    mode_declare(I, any, AI, any, P, any, m, integer, n, integer),

    m : length(transpose(A)[1]),
    n : length(A[1]),

     if not m = n then
     (
         print(A, " is not a square matrix"),
         []
     ),

     if not m = rank(A) then
     (
         print(A, " is not a non-singular matrix"),
         []
     ),

    I : copymatrix(ident(m)),
    AI : addcol(A, I),
    print(AI),
    kaidan(AI),
    P : matblock(AI, 1, m, n + 1, n + m),
    P
)$


/* 階段行列に変形(結果だけ求める) */
/* 破壊的変形 */

quiet_kaidan_findnonzero(A, pi, pj, mm) := block([pix],
    mode_declare(pix, integer),
    pix : pi,
    jump,
    if not A[pix, pj] = 0 then
        pix
    else if pix = mm then
        (-1)
    else
    (
       pix : pix + 1,
       go(jump)
    )
)$



quiet_kaidan(A) := block([m, n, i, j, ir],
    mode_declare(m, integer, n, integer, i, integer, j, integer, ir, integer),

    if matrixp(A) = false then
    (
        print(A," is not a matrix"),
        []
    ),

    m : length(transpose(A)[1]),
    n : length(A[1]),

    i : 1,
    j : 1,
    
    loop,
    
    ir : quiet_kaidan_findnonzero(A, i, j, m),
    if ir = -1 then
    (
        if j = n then
            go(finish)
        else
        (
            j : j + 1,
            go(loop)
        )
    )
    else if ir > i then
    (
        col_ex(A, i, ir)
    ),

    col_sp(A, 1 / A[i, j], i),

    if i > 1 then
        for k : 1 thru i - 1 do
            if not A[k, j] = 0 then
            (
                col_ad(A, -A[k, j],  k,  i)
            ),

    if i < m then
        for k : i + 1 thru m do
            if not A[k, j] = 0 then
            (
                col_ad(A, -A[k, j], k, i)
            ),

    if i < m and j < n then
    (
       i : i + 1,
       j : j + 1,
       go(loop)
    ),
    
    finish,

    A
)$



/* 行列 A を階段行列に変形する正則行列 P を求める */

/* PA : echelon */


queryp_findnonzero(A, pi, pj, mm) := block([pix],
    mode_declare(pix, integer),
    pix : pi,
    jump,
    if not A[pix, pj] = 0 then
        pix
    else if pix = mm then
        (-1)
    else
    (
       pix : pix + 1,
       go(jump)
    )
)$


queryp(A) := block([P, CPA, m, n, i, j, ir, dummy, c],
    mode_declare(P, any, CPA, any, m, integer, n, integer,
                 i, integer, j, integer, ir, integer,
                 dummy, any, c, any),
    

    if matrixp(A) = false then
    (
        print(A," is not a matrix!"),
        []
    ),

    m : length(transpose(A)[1]),
    n : length(A[1]),


    P : copymatrix(ident(m)),
    CPA : copymatrix(A),

    i : 1,
    j : 1,

    loop,

    ir : queryp_findnonzero(CPA, i, j, m),
    if ir = -1 then
    (
        if j = n then
            go(finish)
        else
        (
            j : j + 1,
            go(loop)
        )
    )
    else if ir > i then
    (
        dummy : CPA[_i],
        CPA[i] : CPA[ir],
        CPA[ir] : dummy, 
        dummy : P[i],
        P[i] : P[ir],
        P[ir] : dummy,
        print(CPA, "    ", P)
    ),

    c : 1 / CPA[i, j],
    CPA[i] : c * CPA[i],
    P[i] : c * P[i],
    print(CPA, "    ", P),

    if i > 1 then
        for k : 1 thru i - 1 do
            if not CPA[k, j] = 0 then
            (
                c : CPA[k, j],
                CPA[k] : CPA[k] - c * CPA[i],
                P[k] : P[k] - c * P[i],
                print(CPA, "    ", P)
            ),

    if i < m then
        for k : i + 1 thru m do
            if not CPA[k, j] = 0 then
            (
                c : CPA[k, j],
                CPA[k] : CPA[k] - c * CPA[i],
                P[k] : P[k] - c * P[i],
                print(CPA, "    ", P)
            ),

    if i < m and j < n then
    (
       i : i + 1,
       j : j + 1,
       go(loop)
    ),

    finish,

    P
)$



/* 階段行列を標準形に変形する正則行列 Q を求める。 */

/* A : echelon ,  AQ : canonical */

queryq_findfirst(A, ii, nn) := block([ij, l],
    mode_declare(ij, integer, l, integer),
    for l : ii thru nn do
        if (A[ii, l] = 1) then
            ij : l
        else
                ij : l + 1,
    if ij <= nn then
        return(ij)
    else
        return(-1)
)$


queryq(A) := block([m, n, minima, Q, CPA],
    mode_declare(m, integer, n, integer, minima, integer, Q, any, CPA, any),
    if matrixp(A) = false then
    (
        print(A, " is not a matrix!"),
        []
    ),
    n : length(A[1]),
    m : length(transpose(A)[1]),
    Q : copymatrix(ident(n)),
    CPA : copymatrix(A),
   
    if m > n then
        minima : n
    else
        minima : m,
    
    block([i, j, k],
        mode_declare(i, integer, j, integer, k, integer),
        for i : 1 thru minima do
        (
            j : queryq_findfirst(CPA, i, n),
            if (not j = -1) and (not i = j) then
            (
                row_ex(CPA, i, j),
                row_ex(Q, i, j),
                print(Q)
            ),
            if i < n then
                for k : i + 1 thru n do
                    if not CPA[i, k] = 0 then
                    (
                        row_ad(Q, -CPA[i, k], k, i),
                        row_ad(CPA, -CPA[i, k], k, i),
                        print(Q)
                    )
        )
    ),
    Q
)$


getpq(A) := block([P, Q, B, C],
    mode_declare(P, any, Q, any, B, any, C, any),
    B : copymatrix(A),
    C : quiet_kaidan(B),
    P : queryp(A),
    Q : queryq(C),
    [P, Q]
)$



/* 2008/6/5 追加 */
/* 正方行列を上三角行列に基本変形 */
/* ある行に他のある行の定数倍を加えるという操作だけを使う。*/

/* 2008/6/5 */
/* transforms to an upper-triangulate matrix */
/* only use A[i] ---> A[i] + k * A[j] */

/* 破壊的　*/

kamisankaku_findnonzero(A, pi, pj, mm) := block([pix],
    mode_declare(pix, integer),
    pix : pi,
    jump,
    if not A[pix, pj] = 0 then
        pix
    else if pix = mm then
        (-1)
    else
    (
        pix : pix + 1,
        go(jump)
    )
)$


kamisankaku(A) := block([m, n, i, ir],
    mode_declare(m, integer, n, integer, i, integer, ir, integer),
    if matrixp(A) = false then
    (
        print(A," is not a matrix"),
        []
    ),

    m : length(transpose(A)[1]),
    n : length(A[1]),


    if not m = n then
    (
        print(A, " is not a square-matrix"),
        []
    ),

    i : 1,

    loop,

    if i = m then go(finish),

    if not A[i, i] = 0 then
    (
        for k : i + 1 thru m do
            if not A[k, i] = 0 then
            (
                col_ad(A, -(A[k, i]/A[i, i]), k, i),
                print(A)
             )
    )
    else
    (
        ir : kamisankaku_findnonzero(A, i, i, m),
        if ir = -1 then
        (
            i : i + 1,
            go(loop)
        )
        else
        (
            col_ad(A, 1, i, ir),
            print(A),
            for k : i + 1 thru m do
                if not A[k, i] = 0 then
                (
                    col_ad(A, -(A[k, i]/A[i, i]), k, i),
                    print(A)
                )
        )
    ),

    if i < m then
    (
        i : i + 1,
        go(loop)
    ),

    finish,

    A
)$

/* 非破壊的 */

cp_kamisankaku(A) := block([TA],
    mode_declare(TA, any),
    TA : copymatrix(A),
    kamisankaku(TA)
)$
 

/* 正方行列の行列式を求める。 */
/* 上三角行列に変形して、対角成分の積を取る。*/
/* 非破壊的 */
/* detreminant  */


gyouretsushiki(A) := block([B, detm, m, n],
    mode_declare(B, any, detm, any, m, integer, n, integer),
    B : copymatrix(A),
    m : length(transpose(B)[1]),
    n : length(B[1]),
    if not m = n then
    (
        print(A, " is not a square-matrix"),
        []
    ),
    kamisankaku(B),
    detm : prod(B[k, k], k, 1, m),
    detm
)$



/* LU分解 */
/* LU resolution */

luresolution_findnonzero(A, pi, pj, mm) := block([pix],
    mode_declare(pix, integer),
    pix : pi,
    loop1,
        if not A[pix, pj] = 0 then
            pix
        else if pix = mm then
            -1
        else
        (
            pix : pix + 1,
            go(loop1)
        )
)$


luresolution(A) := block([P, D, L, LU, I, m, n, i, ir],
    mode_declare(P ,any, D, any, L, any, LU, any, I, any,
                 m, integer, n, integer, i, integer, ir, integer), 
    if not matrixp(A) then
    (
        print(A, " is not a matrix"),
        []
    ),
    m : length(transpose(A)[1]),
    n : length(A[1]),
    if not m = n then
    (
        print(A, " is not a square-matrix"),
        []
    ),
    P : ident(n),
    i : 1,
    loop,
        if i = m then go(finish),
        if not A[i, i] = 0 then
        (
            for k : i + 1 thru m do
                if not A[k, i] = 0 then
                (
                    col_ad(P, -(A[k, i]/A[i, i]), k, i),
                    col_ad(A, -(A[k, i]/A[i, i]), k, i)
                )
        )
        else
        (
            ir : _luresolution_findnonzero(A, i, i, m),
            if ir = -1 then
            (
                i : i + 1,
                go(loop)
            )
            else
            (
                col_ad(P, 1, i, ir),
                col_ad(A, 1, i, ir),
                for k : i + 1 thru m do
                    if not A[k, i] = 0 then
                    (
                        col_ad(P, -(A[k, i]/A[i, i]), k, i),
                        col_ad(A, -(A[k, i]/A[i, i]), k, i)
                    )
            )
        ),
        if i < m then
        (
            i : i + 1,
            go(loop)
        ),
    finish,
    I : ident(n),
    D : addcol(P, I),
    quiet_kaidan(D),
    L : matblock(D, 1, n, n + 1, n + n),
    LU : [L, A],
    LU
)$


cp_luresolution(A) := block([B],
    mode_declare(B, any),
    B : copymatrix(A),
    luresolution(B)
)$


/* alias */


lurslv(A) := luresolution(A)$
cp_lurslv(A) := cp_luresolution(A)$

lfctr(A) := first(cp_luresolution(A))$
ufctr(A) := first(rest(cp_luresolution(A)))$



/* 対称行列の対角化 */

/* List ===> Matrix */

listtomatrix(a) := block([tv, m, b],
    mode_declare(tv, list, m, integer, b, list),
    if listp(a) then
    (
        b : copylist(a),
        m : length(a),
        tv : matrix(first(b)),
        b : rest(b),

        for i : 1 thru m - 1 do
            tv : addrow(tv, b[i]),
        tv
    )
    else
        []
)$


/* Matrix ===> List */

matrixtolist(a) := block([tl, m],
    mode_declare(tl, list, m, integer),
    if matrixp(a) then
    (
        tl : [],
        m : length(a),
        for i : m thru 1 step -1 do
            tl : cons(a[i], tl),
        tl
    )
    else
        []
)$



/* alias */


l2m(a) := listtomatrix(a)$

m2l(a) := matrixtolist(a)$


/*********************************************************************/
/*           eigen values, eigen vectors, diagonalization            */
/*********************************************************************/
/* require load("eigen");                                            */                                     
/* 2014/7/30                                                         */
/* The functions below don't work maxima-5.9.0 and the laters.       */
/*These only work well maxima-4.155                                  */
/*********************************************************************/
/*                                                                   *
 * diagonalizer(A) := block([B, AL],                                 *
 *     mode_declare(B, any, AL, list),                               *
 *     AL : maplist(lambda([x], unitvector(x)),                      *
 *                      gramschmidt(rest(eigenvectors(A)))),         *
 *     B : l2m(AL),                                                  *
 *     ratsimp(B)                                                    *
 * )$                                                                *
 **********************************************************************/
/****************************************************************************
 *      Old Maxima's eigenvectors returns    5.4.155 ... 5.9.3              *
 *                                                                          *
 *      [ [[eigenvalues],[multiplicities]],  [   ],[   ],...,[   ]  ]       *      
 *                                           sequence of eigen vectors      *
 *                                                                          *
 *      Modern Maxima's eigenvectors returns 5.9.39 ...                     *
 *                                                                          *
 *      [ [[eigenvalues],[multiplicities]],  [[   ]],[[  ]],...,[[   ]] ]   *
 *                                           sequence of lists of 1-length  *
 *                                           including eigenvectors         *
 *                                                                          *
 ****************************************************************************/

/* New version of diagonalizer for Maxima-5.9.39 or laters                  */
/* 2021/11/23  */

/* 対称行列 "A" に対して PAP^t が対角行列となるような "P" を求める。 */

diagonalizer(A) := block([TL0, TL1, TL2, TL3, n, P],
    mode_declare(TL0, list, TL1, list, TL2, list, TL3, list, n, integer, P, any),
    TL0 : first(rest(eigenvectors(A))),
    TL1 : [],
    n : length(TL0),
    for i:1 thru n do
    (
        TL1 : cons(first(first(TL0)), TL1),
        TL0 : rest(TL0)
    ),
    TL2 : gramschmidt(TL1),
    TL3 : maplist(lambda([x], unitvector(x)), TL2),
    P : l2m(TL3),
    ratsimp(P)
)$
 

/* 対角化された行列 PAP^t を返す */ 
 
diagonalize(A) := block([P],
    mode_declare(P, any),
    P : diagonalizer(A),
    ratsimp(ratsimp(P) . A . ratsimp(transpose(P)))
)$


/*****************************************************************************
 * unitarize(A) := block([AL],
 *     mode_declare(AL, list),
 *     AL : maplist(lambda([x], unitvector(x)),
 *                       gramschmidt(m2l(transpose(A)))),
 *   ratsimp(transpose(l2m(AL)))
 *)$
 ******************************************************************************/

/* CCaution! "A" must be an Hermitian matrix.                                 *
 * This function doesn't check the matrix type of the argument                */


 /* Hermite 行列 A をユニタリ化する。 */
 
 unitarize(A) := block([AL, TL, GL, TA],
    mode_delare(AL, list, TL, list, GL, list, TA, any),
    TL : m2l(transpose(A)),
    GL : gramschmidt(TL),
    AL : maplist(lambda([x], unitvector(x)), GL),
    TA : transpose(l2m(AL)),
    ratsimp(TA)
)$


/******************************************************************************
 * gson(AL) := maplist(lambda([x], unitvector(x)), gramschmidt(AL))$
 ******************************************************************************/
 

gson(AL) := block([TL, GL],
    mode_declare(TL, list, GL, list),
    TL : copylist(AL),
    GL : gramschmidt(TL),
    TL : maplist(lambda([x], unitvector(x)), GL),
    ratsimp(TL)
)$

/***** bottom ******/