/* kihon.v */

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

rowvector(A) := matrix(A)$

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

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

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

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

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

cp_row_sp(A, k, i) := block([B],
    mode_declare(B, matrix),
    B : cp_col_sp(transpose(A), k, i),
    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],
    mode_declare(len, fixnum),
    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, fixnum),
    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, fixnum, 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
)$

kaidan_findnonzero(A, pi, pj, mm) := block([pix],
    mode_declare(pix, fixnum),
    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, fixnum, n, fixnum, i, fixnum, j, fixnum, ir, fixnum),

    if not matrixp(A) 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)
)$

k2h_findfirst(A, ii, nn) := block([ij],
    mode_declare(ij, fixnum),
    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, fixnum, n, fixnum, minima, fixnum),
    if  not matrixp(A) 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, fixnum),
        for i : 1 thru minima do
        (
            j : k2h_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, any),
    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],
    mode_declare(A, any, m, fixnum, n, fixnum),
    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
)$

matreplace(Mat1, Mat2, i1, i2, j1, j2) := block([A, m, n],
    mode_declare(A, any, m, fixnum, n, fixnum),
    m : i2 - i1,
    n : j2 - j1,
    A : copymatrix(Mat1),
    for i : 1 thru m + 1 do
        for j : 1 thru n + 1 do
            A[i1 + i - 1][j1 + j - 1] : Mat2[i][j],
    A
)$


myinv(A) := block([I, CPA, AI, P, m, n],
    mode_declare(I, any, CPA, any, AI, any, P, any, m, fixnum, n, fixnum),
    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, "in not a non-singular matrix1"),
        []
    ),
    CPA : copymatrix(A),
    I : copymatrix(ident(m)),
    AI : addcol(CPA, I),
    print(AI),
    kaidan(AI),
    P : matblock(AI, 1, m, n + 1, n + m),
    P
)$


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

queryp(A) := block([P, CPA, m, n, i, j, ir, dummy, c],
    mode_declare(P, any, CPA, any, m, fixnum, n, fixnum,
                 i, fixnum, j, fixnum, ir, fixnum, dummy, any, c, any),
    if not matrixp(A) 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 : qp_findnonzero(CPA, i, j, m),
        if ir = -1 then
        (
            if j = n then
                go(finish)
            else
            (
                j : j + 1,
                go(loop)
            )
        )
        else if ir > 1 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
)$


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


qaidan(A) := block([m, n, i, j, ir],
    mode_declare(m, fixnum, n, fixnum, i, fixnum, j, fixnum, ir, fixnum),

    if not matrixp(A) then
    (
        print(A, " is not a matrix!"),
        []
    ),
    m : length(transpose(A)[1]),
    n : length(A[1]),
    i : 1,
    j : 1,
    loop,
        ir : qk_findnonzero(A, i, j, m),
        if ir = -1 then
        (
            if j = n then
                go(finish)
            else
            (
                j : j + 1,
                go(loop)
            )
        )
        else if ir > 1 then
            col_ex(A, i, ir),
        if not A[i, j] = 1 then
            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
)$


qq_findfirst(A, ii, nn) := block([ij],
    mode_declare(ij, fixnum),
    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, fixnum, n, fixnum, minima, fixnum, Q, any, CPA, any),
    if not matrixp(A) 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([j],
        mode_declare(j, fixnum),
        for i : 1 thru minima do
        (
            j : qq_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 : qaidan(B),
    P : queryp(A),
    Q : queryq(C),
    [P, Q]
)$


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

kamisankaku(A) := block([m, n, i, ir],
    mode_declare(m, fixnum, n, fixnum, i, fixnum, ir, fixnum),
    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!"),
        []
    ),
    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 : ks_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([CPA],
    mode_declare(CPA, any),
    CPA : copymatrix(A),
    kamisankaku(CPA)
)$


lu_findnonzero(A, pi, pj, mm) := block([pix],
    mode_declare(pix, fixnum),
    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, fixnum, n, fixnum, i, fixnum, ir, fixnum),
    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 : copymatrix(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 : lu_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 : copymatrix(ident(n)),
    D : addcol(P, I),
    qaidan(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)
)$

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

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


gyouretsushiki(A) := block([B, detm, m, n],
    mode_declare(B, any, detm, any, m, fixnum, n, fixnum),
    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
)$


mydet(A) := gyouretsushiki(A)$


listtomatrix(a) := block([tv, m, b],
    mode_declare(tv, any, m, fixnum, 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
        []
)$


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


ltom(a) := listtomatrix(a)$
mtol(a) := matrixtolist(a)$


diagonalizer(A) := block([B, AL],
    mode_declare(B, any, AL, list),
    AL : maplist(lambda([x], unitvector(x)),
                 gramschmidt(rest(eigenvectors(A)))),
    B : ltom(AL),
    ratsimp(B)
)$


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(mtol(transpose(A)))),
    ratsimp(transpose(ltom(AL)))
)$


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


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


qamisankaku(A) := block([m, n, i, ir],
    mode_declare(m, fixnum, n, fixnum, i, fixnum, ir, fixnum),
    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!"),
        []
    ),
    i : 1,
    loop,
        if i = m then
            go(finish),
        if not A[i, i] = 0 then
            for k : i + 1 thru m do
                col_ad(A, -(A[k, i]/A[i, i]), k, i)
        else
        (
            ir : qs_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)
            )
        ),
        if i < m then
        (
            i : i + 1,
            go(loop)
        ),
    finish,
    A
)$