exception MatrixNotInvertible

(* Approximation of zero *)                
let leps l = (abs_float l) < epsilon_float
        
                
open Array

(* Dot product of two vectors a and b *)
let vdot a b = 
        fold_left (+.) 0.0 (mapi (fun i x -> x *. b.(i)) a)
        
(* Add two vectors a and b together *)        
let vadd a b =
        mapi (fun i x -> x +. b.(i)) a
        
(* Subtract vector b from a*)        
let vsub a b =
        mapi (fun i x -> x -. b.(i)) a

(* Negate vector a *)
let vneg a =
        map (fun x -> (-.x)) a
        
(* Scale a vector a with a scalar float k *)        
let vscale k a =
        map (fun x -> k *. x) a
        
(* Multiply a (square) matrix and a vector (a) with the same length as the length of a side of the matrix m *)        
let mvmul m a =
        map (fun x -> (vdot x a)) m        

(* Multiply a vector and a matrix *)        
let vmmul a m =        mvmul m a        

(* Add two matrices of the same rank *)
let madd m1 m2 =
        mapi (fun i r -> (mapi (fun j c -> m1.(i).(j) +. m2.(i).(j)) r) ) m1

(* Subtract two matrices of the same rank *)
let msub m1 m2 =
        mapi (fun i r -> (mapi (fun j c -> m1.(i).(j) -. m2.(i).(j)) r) ) m1

(* Multiply two (not necessarily square, as long as the width of m1 equals the height of m2) matrices *)
let mmmul m1 m2 = 
        let col m i = map (fun x -> x.(i) ) m in
        map (fun r -> mapi (fun i _ -> (vdot r (col m2 i))) r) m1
        
(* Transpose a matrix *)        
let mtrans m =        
        let col m i = map (fun x -> x.(i) ) m in
        mapi (fun i _ -> col m i) m 
        
(* Scale matrix m with scalar float k *)        
let mscale k m =
        map (fun r -> map (fun x -> k *. x) r) m

(* Assistance function: returns a matrix m with row i and column j removed *)
let minor m i j =
        let mrow r i = append (sub r 0 i) (sub r (i+1) ((length r) - i - 1)) in
        let m' = mrow m i in
        map (fun x -> mrow x j) m'                

(* Assistance function: returns 1 if i is even, otherwise -1 *)
let fi i = if ((i land 1) = 0) then (1.0) else (-.1.0)
                
(* Returns the determinant of a square matrix *)                
let rec mdet m =
        if ((length m) = 1) then m.(0).(0) else
        if ((length m) = 2) then m.(0).(0) *. m.(1).(1) -. m.(0).(1) *. m.(1).(0) else
        fold_left (+.) 0.0 (mapi (fun i _ -> (fi i) *. m.(0).(i) *. (mdet (minor m 0 i)))  m)
        
(* Calculate the inverse of an invertible square matrix. Raises MatrixNotInvertible if the determinant is (close to) zero *)        
let minverse m =
        let d = mdet m in if (leps d) then raise MatrixNotInvertible else
        let d' = 1.0 /. d in
        (mscale d' (mtrans (mapi (fun i r -> (mapi (fun j c -> (fi (i+j)) *. mdet (minor m i j)) r)) m)))