/* kihon.mac */
/* (c) Kenrou Adachi, 2005/10/29, 2007/7/1, 2008/6/5 */
/* 行列の基本変形パッケージ */

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

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


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


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


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


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


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


/* 破壊的変形 */

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, p],
	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, p],
	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, p],
	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
)$


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

/* 破壊的変形 */

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

    if matrixp(A) = FALSE then
    (
        print(A," is not a matrix"),
        return(FALSE)
    ),


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

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

    i : 1,
    j : 1,
    
    loop,
    
    ir : 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)
    ),

    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],
  CPA : copymatrix(A),
  kaidan(CPA)
)$



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

kai2hyo(A) := block([m, n, minima],

    if matrixp(A) = FALSE then
    (
        print(A, " is not a matrix!"),
        return(FALSE)
    ),

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

    if m > n then
        minima : n
    else
        minima : m,
    
    findfirst(A, ii, nn) := block([ij, l],
        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)
    ),

    block([i, j, k],
        for i : 1 thru minima do
        (
            j : 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],
    CPA : copymatrix(A),
    kai2hyo(CPA)
)$


/* 行列を標準形に */

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


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

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


/* 行列の切りだし */

matblock(Mat, i1, i2, j1, j2) := block([A, m, n, i, j],
    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 の逆行列を計算過程を表示して求める。 */

myinv(A) := block([I, A, P, m, n],

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

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

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

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


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


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

    if matrixp(A) = FALSE then
    (
        print(A," is not a matrix!"),
        return(FALSE)
    ),

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

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

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

    i : 1,
    j : 1,

    loop,

    ir : 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
)$



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

    if matrixp(A) = FALSE then
    (
        print(A," is not a matrix"),
        return(FALSE)
    ),


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

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

    i : 1,
    j : 1,
    
    loop,
    
    ir : 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
)$


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

queryq(A) := block([m, n, minima, Q, CPA],

    if matrixp(A) = FALSE then
    (
        print(A, " is not a matrix!"),
        return(FALSE)
    ),

    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,
    
    findfirst(A, ii, nn) := block([ij, l],
        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)
    ),

    block([i, j, k],
        for i : 1 thru minima do
        (
            j : 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],
    B : copymatrix(A),
    C : quiet_kaidan(B),
    P : queryp(A),
    Q : queryq(C),
    [P, Q]
)$


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


kamisankaku(A) := block([m, n, i, ir],

    if matrixp(A) = FALSE then
    (
        print(A," is not a matrix"),
        return(FALSE)
    ),

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

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

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

    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 : findnonzero(A, i, i, m),
        if ir = -1 then
        (
            i : i + 1,
            go(loop)
        )
        else
        (
            col_ad(A, 1, i, ir),
            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
)$


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


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