package dynsim;
import java.lang.Math;

// A class that supports basic array operations.
// Follows matlab conventions, e.g. vectors are nx1 arrays.
// Since Java has built-in array bounds checks, no checks are given here. 
public class Mat {

  // Basic matrix multiply, result = a*b
  public static void Mult(float a[][], float b[][], float result[][]) {
    int rows = a.length;
    int cols = b[0].length;
    int inside = b.length;
    for (int i = 0; i < rows; i++) {
      for (int k = 0; k < cols; k++) {
	float sum = 0.0f;
	for (int j = 0; j < inside; j++) {
	  sum = sum + a[i][j] * b[j][k];
	}
	result[i][k] = sum;
      }
    }
  }

  // Compute the transpose of a, place into result[][]
  public static void Transpose(float a[][], float result[][]) {
    int rows = a.length;
    int cols = a[0].length;
    for (int i = 0; i < rows; i++) {
      for (int j = 0; j < cols; j++) {
	result[j][i] = a[i][j];
      }
    }
  }

  // Scale the matrix a by the scalar argument, return in result[][]
  public static void Scale(float scalar, float a[][], float result[][]) {
    int rows = Math.min(a.length, result.length);
    int cols = Math.min(a[0].length, result[0].length);
    for (int i = 0; i < rows; i++) {
      for (int j = 0; j < cols; j++) {
	result[i][j] = scalar * a[i][j];
      }
    }
  }

  // Scalar-weighted addition of a and b, result = a*as + b*bs
  public static void Wadd(float as, float a[][], float bs, float b[][], float result[][]) {
    int rows = a.length;
    int cols = a[0].length;
    for (int i = 0; i < rows; i++) {
      for (int j = 0; j < cols; j++) {
	result[i][j] = as * a[i][j] + bs * b[i][j];
      }
    }
  }

  // Convert the 3x1 vector a into a skew matrix result.
  public static void Skew(float a[][], float result[][]) {
    result[0][0] = 0.0f;
    result[0][1] = - a[2][0];
    result[0][2] = a[1][0];
    result[1][0] = a[2][0];
    result[1][1] = 0.0f;
    result[1][2] = -a[0][0];
    result[2][0] = -a[1][0];
    result[2][1] = a[0][0];
    result[2][2] = 0.0f;
  }

  // Convert from a skew matrix to a 3x1 vector
  public static void Unskew(float a[][], float result[][]) {
    result[0][0] = - a[1][2];
    result[1][0] = a[0][2];
    result[2][0] = - a[0][1];
  }

  // Put an identity matrix into the nxn matrix Ident
  public static void Ident(float result[][]) {
    int size = result.length;
    for (int i = 0; i < size; i++)
      for (int j = 0; j < size; j++)
	result[i][j] =  (i == j) ? 1.0f : 0.0f;
  }

  // Compute the norm (sqrt of sum of squares of the elements) of Ain
  public static float Norm(float Ain[][]) {
    int rows = Ain.length;
    int cols = Ain[0].length;
    float sum = 0.0f;
    for (int i = 0; i < rows; i++)
      for (int j = 0; j < cols; j++)
	sum = sum + Ain[i][j] * Ain[i][j];
    return (float)Math.sqrt(sum);
  }

  // Compute the sum of leading diagonal elements of Ain
  public static float Trace(float Ain[][]) {
    int rows = Ain.length;
    float sum = 0.0f;
    for (int i = 0; i < rows; i++)
      sum = sum + Ain[i][i];
    return sum;
  }

  // Convert the n-vector V to an nx1 matrix M
  public static void VtoM(float V[], float M[][]) {
    int rows = Math.min(V.length, M.length);
    for (int i = 0; i < rows; i++)
      M[i][0] = V[i];
  }

  // Convert the nx1 matrix M to an n-vector V
  public static void MtoV(float M[][], float V[]) {
    int rows = Math.min(V.length, M.length);
    for (int i = 0; i < rows; i++)
      V[i] = M[i][0];
  }
}