/*
** matvec.c: matrix and vector operations

** This is a small, hastily coded Matrix and Vector manipulation set by
** Wayne Hayes, for csc350.
** WARNING: some (most?) of the routines in this library are horribly,
** dreadfully TERRIBLY inefficient, to the point of passing and returning
** entire matrices on the stack BY VALUE.  These were written for ease of
** use on *small* problems, not to be horrendously fast on big ones.
** So BEWARE and don't complain to me when it takes forever to do things
** to measly 100x100 matrices.
*/

/*
** Vectors and Matrices stuff
*/
const N := 20  % the maximum length of vectors and rows of matrices.

% A vectors's actual length is in position zero; a matrices rows are
% stored in A(0)(0)

type vector : array 0 .. N of real
type matrix : array 0 .. N of vector

% The Zero matrix and vector
var tmpmat : matrix
tmpmat (0) (0) := N
for i : 1 .. N
    tmpmat (i) (0) := N
    for j : 1 .. N
	tmpmat (i) (j) := 0
    end for
end for
const mat0 := tmpmat
const vec0 : vector := tmpmat (1)

% The Identity Matrix
tmpmat := mat0
for i : 1 .. N
    tmpmat (i) (i) := 1.0
end for
const Id := tmpmat


% dot product of a and b
function Dot (a, b : vector) : real
    pre a (0) = b (0)
    var sum := 0.0
    for i : 1 .. floor (a (0))
	sum += a (i) * b (i)
    end for
    result sum
end Dot


% Magnitude of v
function Mag (v : vector) : real
    result sqrt (Dot (v, v))
end Mag

% Scalar multiplication of k with v
function SVec (k : real, v : vector) : vector
    var u := v
    for i : 1 .. floor (v (0))
	u (i) *= k
    end for
    result u
end SVec


% Scalar multiplication of k with A
function SMat (k : real, A : matrix) : matrix
    var B : matrix
    B (0) (0) := A (0) (0)
    for i : 1 .. floor (A (0) (0))
	B (i) := SVec (k, A (i))
    end for
    result B
end SMat


% Matrix-vector multiplication
function MVec (A : matrix, x : vector) : vector
    pre A (1) (0) = x (0)
    var b : vector
    b (0) := A (0) (0)
    for i : 1 .. floor (A (0) (0))
	b (i) := 0.0
	for j : 1 .. floor (x (0))
	    b (i) += A (i) (j) * x (j)
	end for
    end for
    result b
end MVec


% Matrix transpose
function Transpose (A : matrix) : matrix
    var At : matrix
    At (0) (0) := A (1) (0)
    for i : 1 .. floor (A (1) (0))
	At (i) (0) := A (0) (0)
	for j : 1 .. floor (At (i) (0))
	    At (i) (j) := A (j) (i)
	end for
    end for
    result At
end Transpose


% Matrix-Matrix multiplication
function MM (A, B : matrix) : matrix
    pre A (1) (0) = B (0) (0)
    var prod : matrix
    prod (0) (0) := A (0) (0)
    for i : 1 .. floor (A (0) (0))
	prod (i) (0) := B (1) (0)
	for j : 1 .. floor (B (1) (0))
	    prod (i) (j) := 0.0
	    for k : 1 .. floor (A (1) (0))
		prod (i) (j) += A (i) (k) * B (k) (j)
	    end for
	end for
    end for
    result prod
end MM

% aX + bY, a,b scalars, X,Y vectors
function AddVec (a : real, x : vector, b : real, y : vector) : vector
    pre x (0) = y (0)
    var sum : vector
    sum (0) := x (0)
    for i : 1 .. floor (x (0))
	sum (i) := a * x (i) + b * y (i)
    end for
    result sum
end AddVec


% Are a and b equal to within tolerance?
function EqVec (a, b : vector, tolerance : real) : boolean
    pre a (0) = b (0)
    for i : 1 .. floor (a (0))
	if abs (a (i) - b (i)) > tolerance then
	    result false
	end if
    end for
    result true
end EqVec


% Are A and B equal to tolerance?
function EqMat (A, B : matrix, tolerance : real) : boolean
    pre A (0) (0) = B (0) (0)
    for i : 1 .. floor (A (0) (0))
	if not EqVec (A (i), B (i), tolerance) then
	    result false
	end if
    end for
    result true
end EqMat


% Make a unit direction vector
function UnitVec (r : vector) : vector
    pre not EqVec (r, vec0, 0)
    var ru : vector
    ru (0) := r (0)
    var mag := Mag (r)
    for i : 1 .. floor (r (0))
	ru (i) := r (i) / mag
    end for
    result ru
end UnitVec


% Output a vector
proc PutVec (a : vector)
    for i : 1 .. floor (a (0))
	put a (i) : 11 ..
    end for
    put ""
end PutVec

% Output a matrix
proc PutMat (M : matrix)
    for i : 1 .. floor (M (0) (0))
	PutVec (M (i))
    end for
end PutMat

% Input a vector
proc GetVec (var a : vector)
    for i : 1 .. floor (a (0))
	get a (i)
    end for
end GetVec

% Input a matrix: Need M(0)(0) set to #rows, M(1)(0) set to #cols
proc GetMat (var M : matrix)
    for i : 1 .. floor (M (0) (0))
	M (i) (0) := M (1) (0)
	GetVec (M (i))
    end for
end GetMat

function RandVec (n : int) : vector
    var v : vector
    v (0) := n
    for i : 1 .. n
	rand (v (i))
	v (i) *= 10
    end for
    result v
end RandVec

function RandMat (m, n : int) : matrix
    var M : matrix
    M (0) (0) := m
    for i : 1 .. m
	M (i) := RandVec (n)
    end for
    result M
end RandMat


% Perform Gram-Schmidt orthogonalization on the columns of A
proc GramSchmidt (var Q, R : matrix, A : matrix)
    % Since matrices in Turing are stored row-major, and Gram-Schmidt is
    % much easier if they're column major, I take Transpose(A) and *pretend*
    % that At is stored column-major.  I also pretend Q is stored column
    % major (R doesn't matter, it's just Rij all the time), and then return
    % Q's transpose at the end.
    const At := Transpose (A)
    Q := mat0
    R := mat0
    const m := floor (A (0) (0))
    const n := floor (A (1) (0))
    Q (0) (0) := n
    R (0) (0) := n
    for i : 1 .. max (m, n)
	Q (i) (0) := m
	R (i) (0) := n
    end for

    for j : 1 .. n
	Q (j) := At (j)
	for i : 1 .. j - 1
	    R (i) (j) := Dot (Q (i), Q (j))
	    Q (j) := AddVec (1, Q (j), - R (i) (j), Q (i))
	end for
	R (j) (j) := Mag (Q (j))
	if R (j) (j) not= 0 then
	    Q (j) := SVec (1 / R (j) (j), Q (j))
	end if
    end for
    Q := Transpose (Q)
end GramSchmidt


% LU Factorization, no pivoting.
proc LUFact (var L, U : matrix, A : matrix)
    pre A (0) (0) = A (1) (0)
    const n := floor (A (0) (0))
    L := Id
    L (0) (0) := n
    L (n) (0) := n
    U := A
    for k : 1 .. n - 1
	L (k) (0) := n
	for i : k + 1 .. n
	    if U (k) (k) not= 0 then
		var c := U (i) (k) / U (k) (k)
		L (i) (k) := c
		for j : k .. n
		    U (i) (j) -= c * U (k) (j)
		end for
	    end if
	end for
    end for
end LUFact


% Forward Substitution.  L must be the L returned from LUFact.  We
% return the solution vector.
function ForwSubst (L : matrix, b : vector) : vector
    var y : vector
    y := b
    for i : 1 .. floor (b (0))
	for j : 1 .. i - 1
	    y (i) -= L (i) (j) * y (j)
	end for
	y (i) := y (i) / L (i) (i)
    end for
    result y
end ForwSubst



% Back substitution.  U is the U matrix returned from GaussElim.
% We return the vector of x's.
function BackSubst (U : matrix, b : vector) : vector
    pre U (0) (0) = U (1) (0) and U (0) (0) = b (0)
    const n := floor (b (0))
    var x : vector
    x := b

    for decreasing i : n .. 1
	for j : i + 1 .. n
	    x (i) -= U (i) (j) * x (j)
	end for
	x (i) := x (i) / U (i) (i)
    end for
    result x
end BackSubst


% Solve using LUFact, Forw/BackSubst, for the lazy type.
function Solve (A : matrix, b : vector) : vector
    var L, U : matrix
    LUFact (L, U, A)
    result BackSubst (U, ForwSubst (L, b))
end Solve

% Determinant of a matrix
function Det (A : matrix) : real
    pre A (0) (0) = A (1) (0)
    var U : matrix
    LUFact (tmpmat, U, A)
    var prod : real := 1.0
    for i : 1 .. floor (A (0) (0))
	prod *= U (i) (i)
    end for
    result prod
end Det

