(************************************************************
   solve.ml		Created      : Mon Mar 25 01:31:45 2002
  			Last modified: Tue Mar 26 11:26:45 2002
  Compile: make #
************************************************************)
(* 連立方程式を解く
 * 正方行列 fabs
 *
 *)
(*open Object.Vector3D*)
type s = Vector of float * float * float
type t = float array array

let print_matrix m =
  Array.iter 
    (fun x -> 
      Array.iter (fun x -> Printf.printf "%3.2f," x) x;
      print_string "\n";) m
  
(* for debug *)
let print_int_array ia =
  begin
    Printf.printf "[| ";
    Array.iter (fun x -> Printf.printf "%2d; " x) ia;
    Printf.printf "|]\n";
  end
(*************)
      
let make_adjoint_matrix3 (Vector(v11,v12,v13))
    (Vector(v21,v22,v23)) (Vector(v31,v32,v33)) (Vector(b1,b2,b3)) =
  [| [| v11; v21; v31; b1 |];
     [| v12; v22; v32; b2 |];
     [| v13; v23; v33; b3 |]
   |]

(*  [| [| v11; v12; v13 |];*)
(*     [| v21; v22; v23 |];*)
(*     [| v31; v32; v33 |];*)
(*     [| b1;  b2;  b3|]*)
(*   |]*)

  (*
     [| [| 11; 21; 31; 1 |];
     [| 12; 22; 32; 2 |];
     [| 13; 23; 33; 3 |] |]
   *)
(* 素朴なガウスの消去法 *)
let solve matrix dim =
  let order = 
    let tmp = Array.make dim 1 in
    begin
      for i = 0 to (Array.length tmp)-1 do
	tmp.(i) <- i
      done;
      tmp;
    end
  in
  let rec sweep row =
    if row = dim then ()
    else
      let rec select_pivot_rel index current_max result =
	if index >= dim then result
	else
	  let current_value = abs_float matrix.(order.(index)).(row)
	  in
	  if current_value > current_max then
	    select_pivot_rel (index+1) current_value index
	  else
	    select_pivot_rel (index+1) current_max result 
      in
      let rec sweep_lines pivot_abs target_rel =
	if target_rel = dim then ()
	else
	  let target_abs = order.(target_rel) 
	  in
	  let ratio = matrix.(target_abs).(row) /. matrix.(pivot_abs).(row)
	  in
	(* 絶対位置で行指定 *)
	  let rec sweep_one_line row = 
	    if row = dim+1 then ()
	    else
	      begin
		matrix.(target_abs).(row) <- 
		  matrix.(target_abs).(row) -. ratio *. matrix.(pivot_abs).(row);
		sweep_one_line (row+1);
	      end	
	  in
	  begin
	    sweep_one_line row;
(*	    let _ = Printf.printf "Sweep row %d------------\n" row in*)
(*	    let _ = print_matrix matrix in*)
(*	    let _ = print_string "------------------------------\n" in*)
	    sweep_lines pivot_abs (target_rel+1);
	  end
      in
      let pivot_rel = select_pivot_rel (row+1)
	  (abs_float matrix.(order.(row)).(row)) row in
      (** debug **)
    (* pivot_rel >= row *)
      begin
(*	let _ = *)
(*	  begin*)
(*	    Printf.printf "order\n  ";*)
(*	    Array.iter (fun x -> Printf.printf "%d," x) order;*)
(*	    print_string "\n";*)
(*	  end in*)
	let pivot_abs = order.(pivot_rel) in
(*	let _ = Printf.printf "pivot_abs=%d\n" pivot_abs in*)
(*	let _ = Printf.printf "pivot_rel=%d\n" pivot_rel in*)
	begin
	  order.(pivot_rel) <- order.(row);
	  order.(row) <- pivot_abs;
	end;
	sweep_lines pivot_abs (row+1);
	sweep (row+1);
      end
  in
  let result_array = Array.make dim 0.0 in
  let rec substitute col_rel =  (* したから順に解を求める *)
    if col_rel = -1 then
      result_array
    else
      let col_abs = order.(col_rel) in
      let b = matrix.(col_abs).(dim) in
      let rec one_solution row result = 
	let current_value = matrix.(col_abs).(row) in
	if row = col_rel then
	  result /. current_value
	else
	  one_solution (row-1) (result -. current_value *. result_array.(row))
      in
      begin
	result_array.(col_rel) <- one_solution (dim-1) b;
	substitute (col_rel-1);
      end
  in
  begin
    sweep 0;
    substitute (dim-1);
  end

(*let _ = *)
(* let v1 = Vector(2.0,3.0,0.0) in*)
(* let v2 = Vector(-3.0,2.0,3.0) in*)
(* let v3 = Vector(1.0,-3.0,1.0) in*)
(* let b = Vector(0.0,11.0,2.0) in*)
(* let m = make_adjoint_matrix3 v1 v2 v3 b in*)
(* print_matrix m;*)
(* Array.iter (fun x -> Printf.printf "%f," x) (solve m 3);*)
(* Printf.printf "\n";*)