/* Call helper functions */
void matrixTranspose(float A[][3], float A_trans[][3]);
void matrixVectorMultiplication(float A[][3], float B[][1], float C[][1]);
void matrixMultiplication(float A[][3], float B[][3], float C[][3]);
void rotationMatrix(float theta, float R[3][3], char axis);

/* Define all your matrices as global variables */
// 3x3 matrices for example
float mat_A[3][3] = {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}};
float mat_B[3][3] = {{9, 8, 7}, {6, 5, 4}, {3, 2, 1}};
float mat_A_trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};  // Initialize
float mat_C[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};    // Initialize
float mat_D[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};    // Initialize


float Rx[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};   // Initialize
float Ry[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};   // Initialize
float Rz[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};   // Initialize

// 3x1 vectors for example
float vec_a[3][1] = {{1}, {2}, {3}};
float vec_b[3][1] = {{0}, {0}, {0}};    // Initialize

// Rotation angles in radians
float theta_x = 0.0;
float theta_y = 0.0;
float theta_z = 0.0;




/* Helper functions to make the code concise
This goes outside of main function */

// For matrix-vector multiplication (3x3 matrix multiplied by 3x1 vector)
// This function gets 3x3 matrix (A) and 3x1 vector (B) as arguments then returns their multiplication C = AB
void matrixVectorMultiplication(float A[][3], float B[][1], float C[][1]) {
    // Initialize result to zero
    for(int i=0; i<3; i++) {
        C[i][0] = 0;
    }

    // Perform multiplication
    for(int i=0; i<3; i++) {
        for(int k=0; k<3; k++) {
            C[i][0] += A[i][k]*B[k][0];
        }
    }
}

// For matrix multiplication (3x3 matrix multiplied by 3x3 matrix)
// This function gets 3x3 matrix (A) and 3x3 matrix (B) as arguments then returns their multiplication C = AB
void matrixMultiplication(float A[][3], float B[][3], float C[][3]) {
    for(int i=0; i<3; i++) {
        for(int j=0; j<3; j++) {
            C[i][j] = 0;
            for(int k=0; k<3; k++) {
                C[i][j] += A[i][k]*B[k][j];
            }
        }
    }
}

// For matrix transpose (3x3 matrix)
// This function gets 3x3 matrix (A) as arguments then returns its transpose A_trans
void matrixTranspose(float A[][3], float A_trans[][3]) {
    // Size of A should be 3x3
    for(int i=0; i<3; i++) {
        for(int j=0; j<3; j++) {
            A_trans[i][j] = A[j][i];
        }
    }
}

// For rotation matrix (3x3 matrix) 
// This function gets a scalar theta and a rotation axis type (x, y, z) then returns a rotational matrix
// that rotates around the rotation axis by theta
void rotationMatrix(float theta, float R[3][3], char axis) {
    if (axis == 'x') {
        R[0][0] = 1;
        R[0][1] = 0;
        R[0][2] = 0;
        R[1][0] = 0;
        R[1][1] = cos(theta);
        R[1][2] = -sin(theta);
        R[2][0] = 0;
        R[2][1] = sin(theta);
        R[2][2] = cos(theta);
    }
    else if (axis == 'y') {
        R[0][0] = cos(theta);
        R[0][1] = 0;
        R[0][2] = sin(theta);
        R[1][0] = 0;
        R[1][1] = 1;
        R[1][2] = 0;
        R[2][0] = -sin(theta);
        R[2][1] = 0;
        R[2][2] = cos(theta);
    }
    else if (axis == 'z') {
        R[0][0] = cos(theta);
        R[0][1] = -sin(theta);
        R[0][2] = 0;
        R[1][0] = sin(theta);
        R[1][1] = cos(theta);
        R[1][2] = 0;
        R[2][0] = 0;
        R[2][1] = 0;
        R[2][2] = 1;
    }
}