/* matrix calculator.  Links with matrix code in alv toolkit. */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "Matrix.h"

/* Static function definitions: */
static double   determ(struct matrix * A);
static int      different(struct matrix * A, struct matrix * B);
static int      issquare(struct matrix * M);
static int      isvec(struct matrix * M);
static int      mpivot(struct matrix * A, struct matrix * B, int row, int col);
static int      samesize(struct matrix * A, struct matrix * B);
static int      samesizevec(struct matrix * A, struct matrix * B);
static struct matrix *findmat(char *name);
static struct matrix *findmat2(char *name);
static struct matrix *getmat(char *name, int r, int c);
static void     Add(void);
static void     Cpy(void);
static void     Create(void);
static void     Delete(void);
static void     Determinant(void);
static void     DProd(void);
static void     Element(void);
static void     Help(void);
static void     Ident(void);
static void     Invert(void);
static void     List(void);
static void     Mult(void);
static void     Norm(void);
static void     parse(void);
static void     Pivot(void);
static void     Print(void);
static void     Quit(void);
static void     Rot(void);
static void     Scale(void);
static void     Set(void);
static void     ShortHelp(void);
static void     Solve(void);
static void     Sub(void);
static void     Translate(void);
static void     Transpose(void);
static void     XProd(void);
static void     XRot(void);
static void     YRot(void);
static void     Zero(void);
static void     ZRot(void);

#define RAD     (3.1415926535/180.)

#ifdef  DOS
#define LINES   18
#else
#define LINES   20
#endif

static Matrix  *matrices = NULL;

#define MALLOC(type,len)        ((type *) malloc(sizeof(type)*(len)))
#define el(A,i,j)               (A->d[(i)*A->c+(j)])
#define isscalar(A)             (A->r == 1 && A->c == 1)

typedef struct {
    char           *cmd;
    void            (*f) ();
    int             nargs;
}               Cmd;

static Cmd      commands[] = {
    {"create", Create, 4},
    {"add", Add, 4},
    {"sub", Sub, 4},
    {"mult", Mult, 4},
    {"print", Print, 2},
    {"delete", Delete, 2},
    {"set", Set, 2},
    {"element", Element, 2},
    {"cp", Cpy, 3},
    {"ident", Ident, 2},
    {"zero", Zero, 2},
    {"transpose", Transpose, 3},
    {"invert", Invert, 2},
    {"dprod", DProd, 4},
    {"xprod", XProd, 4},
    {"norm", Norm, 2},
    {"determinant", Determinant, 3},
    {"solve", Solve, 2},
    {"pivot", Pivot, 4},
    {"rot", Rot, 3},
    {"xrot", XRot, 3},
    {"yrot", YRot, 3},
    {"zrot", ZRot, 3},
    {"scale", Scale, 2},
    {"translate", Translate, 2},
    {"xlate", Translate, 2},
    {"help", Help, 1},
    {"?", ShortHelp, 1},
    {"quit", Quit, 1},
    {"list", List, 1},
};

#define NCMDS   (sizeof(commands)/sizeof(commands[0]))

static char     line[240];
static char    *tokens[40];
static int      ntoken;

/* This cutie is due to Jack Klein */
char *getsafe(char *buffer, int count)
{
   char *result = buffer, *np;
   if ((buffer == NULL) || (count < 1))
      result = NULL;
   else if (count == 1)
      *result = '\0';
   else if ((result = fgets(buffer, count, stdin)) != NULL)
      if (np = strchr(buffer, '\n'))
         *np = '\0';
   return result;
}


int             main(void)
{
    int             i;
    int             len;
    int             nmatch;
    Cmd            *cmd;

    for (;;) {
        printf("> ");
        if (getsafe(line, sizeof line) == NULL)
            exit(EXIT_FAILURE);

        parse();
        if (ntoken > 0) {
            len = strlen(tokens[0]);
            for (nmatch = 0, i = 0; i < NCMDS; ++i) {
                if (strncmp(tokens[0], commands[i].cmd, len) == 0) {
                    cmd = &commands[i];
                    ++nmatch;
                }
            }
            if (nmatch == 0)
                printf("'%s' not found\n", tokens[0]);
            else if (nmatch > 1)
                printf("'%s' is not unique\n", tokens[0]);
            else if (ntoken < cmd->nargs)
                printf("'%s' requires %d arguments\n", cmd->cmd, cmd->nargs - 1);
            else
                cmd->f();
        }
    }
    return 0; /* Not reached... */
}


static void     parse()
{
    char           *ptr = line;

    ntoken = 0;
    while (ntoken < 40 && (tokens[ntoken] = strtok(ptr, " \t,")) != NULL) {
        ++ntoken;
        ptr = NULL;
    }
}

static Matrix  *findmat(char *name)
{
    Matrix         *ptr;

    if (name == NULL)
        return NULL;

    for (ptr = matrices; ptr != NULL; ptr = ptr->next)
        if (strcmp(ptr->name, name) == 0)
            break;

    return ptr;
}


static Matrix  *getmat(char *name, int r, int c)
{
    Matrix         *ptr = findmat(name);
    if (ptr == NULL) {
        ptr = (Matrix *) malloc(sizeof(Matrix));
        ptr->next = matrices;
        ptr->name = name == NULL ? NULL : strdup(name);
        ptr->r = r;
        ptr->c = c;
        ptr->d = MALLOC(double, r * c);
        matrices = ptr;
    } else if (ptr->r != r || ptr->c != c) {
        printf("matrix %s is wrong size (%dx%d)\n", name, ptr->r, ptr->c);
        ptr = NULL;
    }
    return ptr;
}

static Matrix  *findmat2(char *name)
{
    Matrix         *ptr = findmat(name);
    if (ptr == NULL)
        printf("matrix %s not found\n", name);
    return ptr;
}


static int      isvec(Matrix * M)
{
    if (M->r == 1 || M->c == 1)
        return 1;

    printf("%s(%dx%d) must be a vector\n", M->name, M->r, M->c);
    return 0;
}


static int      issquare(Matrix * M)
{
    if (M->r == M->c)
        return 1;

    printf("%s(%dx%d) must be square\n", M->name, M->r, M->c);
    return 0;
}


static int      samesize(Matrix * A, Matrix * B)
{
    if (A->r == B->r && A->c == B->c)
        return 1;

    printf("%s(%dx%d) and %s(%dx%d) must be same size\n",
           A->name, A->r, A->c, B->name, B->r, B->c);
    return 0;
}


static int      samesizevec(Matrix * A, Matrix * B)
{
    if (!isvec(A) || !isvec(B))
        return 0;

    if (A->r * A->c != B->r * B->c) {
        printf("%s(%dx%d) and %s(%dx%d) must be same size\n",
               A->name, A->r, A->c, B->name, B->r, B->c);
        return 0;
    }
    return 1;
}


static int      different(Matrix * A, Matrix * B)
{
    if (A != B)
        return 1;

    printf("cannot use same matrix (%s) for src and dst\n", A->name);
    return 0;
}






static void     Create()
{
    Matrix         *A;

    if (findmat(tokens[1]))
        printf("'%s' already exists\n", tokens[1]);
    else {
        A = getmat(tokens[1], atoi(tokens[2]), atoi(tokens[3]));
        mzero(A);
    }
}

static void     Mult()
{
    Matrix         *A,
                   *B,
                   *C;
    if ((A = findmat2(tokens[1])) && (B = findmat2(tokens[2]))) {
        if (isscalar(A)) {
            if ((C = getmat(tokens[3], B->r, B->c)))
                smult(A, B, C);
        } else if (isscalar(B)) {
            if ((C = getmat(tokens[3], A->r, A->c)))
                smult(B, A, C);
        } else {
            if ((C = getmat(tokens[3], A->r, B->c)) &&
                different(A, C) && different(B, C))
                mmult(A, B, C);
        }
        mprint(C);
    }
}

static void     Print()
{
    Matrix         *M;
    int             i;
    for (i = 1; i < ntoken; ++i)
        if ((M = findmat2(tokens[i])))
            mprint(M);
}

static void     Delete()
{
    Matrix         *A;
    int             i;

    for (i = 1; i < ntoken; ++i)
        if ((A = findmat2(tokens[i])))
            delmat(A);
}

static void     Set()
{
    int             i,
                    n;
    Matrix         *M;

    if ((M = findmat2(tokens[1]))) {
        n = M->r * M->c;
        if (ntoken - 2 < n)
            n = ntoken - 2;
        for (i = 0; i < n; ++i)
            if (strcmp(tokens[i + 2], "-") != 0)
                M->d[i] = atof(tokens[i + 2]);
    }
}

static void     Element()
{
    int             row,
                    col;
    Matrix         *A;

    if ((A = findmat2(tokens[1]))) {
        row = atoi(tokens[2]);
        col = atoi(tokens[3]);
        if (row >= A->r || col >= A->c)
            printf("(%d,%d) is out of bounds for %s(%d,%d)\n",
                   row, col, A->name, A->r, A->c);
        else
            el(A, row, col) = atof(tokens[4]);
    }
}

static void     Cpy()
{
    Matrix         *A,
                   *B;


    if ((A = findmat2(tokens[1])) && (B = getmat(tokens[2], A->r, A->c))) {
        copymat(A, B);
    }
}

static void     Invert()
{
    Matrix         *A,
                   *B;
    if ((A = findmat2(tokens[1])) && issquare(A)) {
        B = A;
        if (ntoken <= 2 || (B = getmat(tokens[2], A->c, A->r))) {
            minvert(A, B);
            mprint(B);
        }
    }
}

static void     Transpose()
{
    Matrix         *A,
                   *B;
    if ((A = findmat2(tokens[1])) && (B = getmat(tokens[2], A->c, A->r)) &&
        different(A, B)) {
        mtranspose(A, B);
        mprint(B);
    }
}

static void     Add()
{
    Matrix         *A,
                   *B,
                   *C;
    if ((A = findmat2(tokens[1])) && (B = findmat2(tokens[2])) &&
        samesize(A, B) && (C = getmat(tokens[3], A->r, A->c))) {
        madd(A, B, C);
        mprint(C);
    }
}

static void     Sub()
{
    Matrix         *A,
                   *B,
                   *C;
    if ((A = findmat2(tokens[1])) && (B = findmat2(tokens[2])) &&
        samesize(A, B) && (C = getmat(tokens[3], A->r, A->c))) {
        msub(A, B, C);
        mprint(C);
    }
}

static void     Ident()
{
    Matrix         *A;
    int             i;

    for (i = 1; i < ntoken; ++i)
        if ((A = findmat2(tokens[i])))
            mident(A);
}


static void     Zero()
{
    Matrix         *A;
    int             i;

    for (i = 1; i < ntoken; ++i)
        if ((A = findmat2(tokens[i])))
            mzero(A);
}


static void     DProd()
{
    Matrix         *A,
                   *B,
                   *C;
    int             i,
                    len;

    if ((A = findmat2(tokens[1])) && (B = findmat2(tokens[2])) &&
        samesizevec(A, B) && (C = getmat(tokens[3], 1, 1))) {
        C->d[0] = 0.;
        len = A->r * A->c;
        for (i = 0; i < len; ++i)
            C->d[0] += A->d[i] * B->d[i];
        mprint(C);
    }
}

static void     XProd()
{
    Matrix         *A,
                   *B,
                   *C;
    int             i,
                    len;

    if ((A = findmat2(tokens[1])) && (B = findmat2(tokens[2])) &&
        samesizevec(A, B)) {
        if ((len = A->r * A->c) < 3)
            printf("cross-product only defined for 1x3 and 1x4 vectors\n");
        else {
            if ((C = getmat(tokens[3], 1, len))) {
                C->d[0] = A->d[1] * B->d[2] - A->d[2] * B->d[1];
                C->d[1] = A->d[2] * B->d[0] - A->d[0] * B->d[2];
                C->d[2] = A->d[0] * B->d[1] - A->d[1] * B->d[0];
                for (i = 3; i < len; ++i)
                    C->d[i] = A->d[i];
            }
        }
    }
}

static void     Norm()
{
    Matrix         *A,
                   *B;
    double          scale;
    int             i,
                    len;

    if ((A = findmat2(tokens[1])) && isvec(A) &&
        (B = getmat(tokens[2], A->r, A->c))) {
        len = A->r * A->c;
        scale = 0.;
        for (i = 0; i < len; ++i)
            scale += A->d[i] * A->d[i];
        if (scale == 0.)
            printf("norm: can't normalize zero vector\n");
        else {
            scale = sqrt(scale);
            for (i = 0; i < len; ++i)
                B->d[i] = A->d[i] / scale;
        }
    }
}

static void     Determinant()
{
    Matrix         *A,
                   *B;


    if ((A = findmat2(tokens[1])) && issquare(A) &&
        (B = getmat(tokens[3], 1, 1))) {
        B->d[0] = determ(A);
    }
}


static void     Pivot()
{
    Matrix         *A,
                   *B;
    int             row,
                    col;
    int             i = 1;

    if ((A = findmat2(tokens[i++]))) {
        if (ntoken > 4)
            B = findmat(tokens[i++]);
        else
            B = NULL;
        row = atoi(tokens[i++]);
        col = atoi(tokens[i++]);
        mpivot(A, B, row, col);
        mprint(A);
        if (B)
            mprint(B);
    }
}


static void     Solve()
{
    Matrix         *A,
                   *B;

    if ((A = findmat2(tokens[1]))) {
        if (ntoken > 2)
            B = findmat(tokens[2]);
        else
            B = NULL;
        msolve(A, A, B);
        mprint(A);
        if (B)
            mprint(B);
    }
}


static void     Rot()
{
    Matrix         *A;
    double          theta;

    if ((A = getmat(tokens[1], 3, 3))) {
        theta = atof(tokens[2]) * RAD;
        mident(A);
        el(A, 0, 0) = el(A, 1, 1) = cos(theta);
        el(A, 1, 0) = -(el(A, 0, 1) = sin(theta));
        mprint(A);
    }
}

static void     XRot()
{
    Matrix         *A;
    double          theta;

    if ((A = getmat(tokens[1], 4, 4))) {
        mident(A);
        theta = atof(tokens[2]) * RAD;
        el(A, 1, 1) = el(A, 2, 2) = cos(theta);
        el(A, 2, 1) = -(el(A, 1, 2) = sin(theta));
        mprint(A);
    }
}

static void     YRot()
{
    Matrix         *A;
    double          theta;

    if ((A = getmat(tokens[1], 4, 4))) {
        mident(A);
        theta = atof(tokens[2]) * RAD;
        el(A, 0, 0) = el(A, 2, 2) = cos(theta);
        el(A, 2, 0) = -(el(A, 0, 2) = sin(theta));
        mprint(A);
    }
}

static void     ZRot()
{
    Matrix         *A;
    double          theta;

    if ((A = getmat(tokens[1], 4, 4))) {
        mident(A);
        theta = atof(tokens[2]) * RAD;
        el(A, 1, 1) = el(A, 0, 0) = cos(theta);
        el(A, 0, 1) = -(el(A, 1, 0) = sin(theta));
        mprint(A);
    }
}

static void     Scale()
{
    Matrix         *A;

    int             i,
                    n;

    n = ntoken - 2 + 1;
    if ((A = getmat(tokens[1], n, n))) {
        mzero(A);
        for (i = 0; i < n - 1; ++i)
            el(A, i, i) = atof(tokens[i + 2]);
        el(A, n - 1, n - 1) = 1.;
        mprint(A);
    }
}

static void     Translate()
{
    Matrix         *A;

    int             i,
                    n;

    n = ntoken - 2 + 1;
    if ((A = getmat(tokens[1], n, n))) {
        mident(A);
        for (i = 0; i < n - 1; ++i)
            el(A, n - 1, i) = atof(tokens[i + 2]);
        el(A, n - 1, n - 1) = 1.;
        mprint(A);
    }
}

static void     Help()
{
    char            buffer[20];
    int             i,
                    j;
    char          **ptr;
    static char    *helptxt[] = {
        "create name r c         create new matrix",
        "set A x x x x ...       set A by rows.  ' - ' to skip entries",
        "element A row col x     set one element in A",
        "add A B C               C = A + B",
        "sub A B C               C = A - B",
        "mult A B C              C = A*B",
        "print A [...]           print matrices",
        "delete A [...]          delete matrices",
        "cp A B                  B = A",
        "ident A [...]           set matrices to I",
        "zero A [...]            set matrices to 0",
        "transpose A B           B = transpose(A)",
        "invert A [B]            B = A**-1",
        "solve A [B]             reduce A in place [and apply to B too]",
        "pivot A [B] row col     pivot A in place [and apply to B too]",
        "dprod A B c             c = dot product of 2 vectors",
        "xprod A B C             C = cross product of 2 vectors",
        "norm A B                B = normalize(A)",
        "determinant A b         b = determinant(A)",
        "rot A a                 create 3x3 rotation matrix, a is degrees",
        "xrot A a                create 4x4 rotation matrix about X axis",
        "yrot A a                create 4x4 rotation matrix about Y axis",
        "zrot A a                create 4x4 rotation matrix about Z axis",
        "scale A s s ...         create scale matrix",
        "translate A x y ..      create translation matrix",
        "help                    this list",
        "?                       short list",
        "list                    list of all defined matrices",
        "quit                    exit",
        "Commands may be given as shortest unique string.  Some",
        "commands allow source and destination to be the same; some",
        "don't.  A scalar is simply a 1x1 matrix; the mult command",
        "treats matrix*scalar multiplication correctly.",
    };

    ptr = helptxt;
    j = 0;
    for (i = 0; i < sizeof(helptxt) / sizeof(helptxt[0]); ++i) {
        if (++j >= LINES) {
            printf("--more--");
            getsafe(buffer, sizeof buffer);
            j = 0;
        }
        printf("%s\n", *ptr++);
    }
}

static void     ShortHelp()
{
    int             i;
    char          **ptr;
    static char    *helptxt[] = {
        "CReate name r c      SEt A x x x x ...  Element A r c x",
        "Add A B C            SUb A B C          Mult A B C",
        "PRint A...           DELete A...        CP A B",
        "Ident A...           ZEro A...          TRANSPose A B",
        "Invert A [B]         SOlve A [B]        PIvot A [B] r c",
        "DProd A B c          XProd A B C        Norm A B",
        "DET A b              Rot A a            XRot A a",
        "Yrot A a             ZRot A a           SCale A s s",
        "TRANSLate A x y      Help               List",
    };

    putchar('\n');
    ptr = helptxt;
    for (i = 0; i < sizeof(helptxt) / sizeof(helptxt[0]); ++i)
        printf("%s\n", *ptr++);
    putchar('\n');
    putchar('\n');
}

static void     Quit()
{
    exit(0);
}

static void     List()
{
    Matrix         *m;

    printf("matrices:\n");
    for (m = matrices; m != NULL; m = m->next)
        printf("  %s(%dx%d)\n", m->name, m->r, m->c);
}





 /* math routines */

copymat(Matrix * A, Matrix * B)
{
    int             i,
                    len = A->r * A->c;

    for (i = 0; i < len; ++i)
        B->d[i] = A->d[i];
}


delmat(Matrix * A)
{
    Matrix         *ptr,
                   *ptr2;


    for (ptr = matrices, ptr2 = NULL; ptr != NULL; ptr2 = ptr, ptr = ptr->next) {
        if (ptr == A) {
            if (ptr2 == NULL)
                matrices = ptr->next;
            else
                ptr2->next = ptr->next;
            if (ptr->name != NULL)
                free(ptr->name);
            free(ptr->d);
            free(ptr);
            break;
        }
    }
}


madd(Matrix * A, Matrix * B, Matrix * C)
{
    int             row,
                    col;

    for (row = 0; row < A->r; ++row)
        for (col = 0; col < B->c; ++col)
            el(C, row, col) = el(A, row, col) + el(B, row, col);
}

msub(Matrix * A, Matrix * B, Matrix * C)
{
    int             row,
                    col;

    for (row = 0; row < A->r; ++row)
        for (col = 0; col < B->c; ++col)
            el(C, row, col) = el(A, row, col) - el(B, row, col);
}

mzero(Matrix * A)
{
    int             row,
                    col;

    for (row = 0; row < A->r; ++row)
        for (col = 0; col < A->c; ++col)
            el(A, row, col) = 0.;
}

mident(Matrix * A)
{
    int             row;

    if (A->r != A->c)
        printf("ident: '%s(%d,%d)' is not square\n", A->name, A->r, A->c);
    else {
        mzero(A);
        for (row = 0; row < A->r; ++row)
            el(A, row, row) = 1.;
    }
}


mmult(Matrix * A, Matrix * B, Matrix * C)
{
    int             row,
                    col,
                    k;
    register double *d = C->d;

    for (row = 0; row < A->r; ++row)
        for (col = 0; col < B->c; ++col) {
            *d = 0;
            for (k = 0; k < A->c; ++k)
                *d += el(A, row, k) * el(B, k, col);
            ++d;
        }
}


 /* scalar multiply */
/* A is really a scalar */
smult(Matrix * A, Matrix * B, Matrix * C)
{
    int             row,
                    col;

    for (row = 0; row < B->r; ++row)
        for (col = 0; col < B->c; ++col)
            el(C, row, col) = A->d[0] * el(B, row, col);
}

mprint(Matrix * A)
{
    int             row,
                    col;

    for (row = 0; row < A->r; ++row) {
        for (col = 0; col < A->c; ++col)
            printf(" %12.6g", el(A, row, col));
        putchar('\n');
    }
    putchar('\n');
}

mtranspose(Matrix * A, Matrix * B)
{
    int             row,
                    col;

    for (row = 0; row < A->r; ++row)
        for (col = 0; col < A->c; ++col)
            el(B, col, row) = el(A, row, col);
}



static double   determ(Matrix * A)
{
    printf("todo\n");
    return 0.;
}



 /* pivot matrix A around specified point. If B is specified, perform same
  * operations on B as on A.  A and B must be the same size. */

static int      mpivot(Matrix * A, Matrix * B, int row, int col)
{
    int             i,
                    j;
    double          scale;

    scale = el(A, row, col);

    /* divide row by this coeff */
    if (scale == 0.) {
        printf("%s is non-invertable\n", A->name);
        return 1;
    }
    if (scale != 1.) {
        scale = 1. / scale;
        for (j = 0; j < A->c; ++j)
            el(A, row, j) *= scale;
        if (B)
            for (j = 0; j < B->c; ++j)
                el(B, row, j) *= scale;
    }
    /* clear out rest of column */
    for (i = 0; i < A->r; ++i)
        if (i != row) {
            scale = el(A, i, col);
            if (scale != 0.)
                for (j = 0; j < A->c; ++j)
                    el(A, i, j) -= el(A, row, j) * scale;
            if (B)
                for (j = 0; j < B->c; ++j)
                    el(B, i, j) -= el(B, row, j) * scale;
        }
    return 0;
}


 /* reduce matrix A with gaussian partial pivoting as far as possible.  If A2
  * is specified, the results are copied back. A2 may be the same as A. If B
  * is specified, perform same operations on B as on A.  A and B must be the
  * same size.  Work is done in temporary matrices so the rows can be sorted
  * when written back. */

int             msolve(Matrix * A, Matrix * A2, Matrix * B)
{
    Matrix         *atmp,
                   *btmp = NULL;
    int             i;
    int             row,
                    col;
    int             rows,
                    cols;
    double          scale;
    int            *indices;

    atmp = getmat(NULL, A->r, A->c);
    copymat(A, atmp);
    if (B) {
        btmp = getmat(NULL, B->r, B->c);
        copymat(B, btmp);
    }
    rows = cols = A->c < A->r ? A->c : A->r;
    indices = MALLOC(int, rows);
    for (i = 0; i < rows; ++i)
        indices[i] = -1;

    for (col = 0; col < cols; ++col) {
        /* find largest coeff in this column in a row that hasn't been done */
        scale = 0.;
        for (i = 0; i < rows; ++i) {
            if (indices[i] == -1 && fabs(el(atmp, i, col)) > fabs(scale)) {
                scale = el(atmp, i, col);
                row = i;
            }
        }
        indices[row] = col;

        if (mpivot(atmp, btmp, row, col))
            return 1;
    }

    /* finally, copy from atmp to A and from btmp to B */
    for (row = 0; row < A->r; ++row) {
        i = indices[row];
        if (A2)
            for (col = 0; col < A->c; ++col)
                el(A2, i, col) = el(atmp, row, col);
        if (btmp)
            for (col = 0; col < B->c; ++col)
                el(B, i, col) = el(btmp, row, col);
    }


    delmat(atmp);
    if (btmp)
        delmat(btmp);
    free(indices);
}


minvert(Matrix * A, Matrix * B)
{
    Matrix         *tmp;

    if (A == B)
        tmp = getmat(NULL, A->r, A->c);
    else
        tmp = B;

    mident(tmp);
    msolve(A, NULL, tmp);
    if (A == B) {
        copymat(tmp, B);
        delmat(tmp);
    }
}