open Lacaml.D

let full_ldl m_A =

  let n = Mat.dim1 m_A in
  assert (Mat.dim2 m_A = n);

  let v_D = Vec.make0 n in
  let m_L = Mat.identity n in

  let compute_d j =
    let l_jk = 
      Mat.copy_row m_L j
    in
    let l_jk__d_k = 
      Vec.mul ~n:(j-1) l_jk v_D
    in
    m_A.{j,j} -. dot ~n:(j-1) l_jk  l_jk__d_k
  in

  let compute_l i j =
    assert (i > j);
    let l_jk__d_k = 
      Mat.copy_row m_L j
      |> Vec.mul ~n:(j-1) v_D
    and l_ik = 
      Mat.copy_row m_L i
    in
    1. /. v_D.{j} *. ( m_A.{i,j} -. dot l_jk__d_k l_ik )
  in

  v_D.{1} <- m_A.{1,1};
  for i=2 to n do
    for j=1 to (i-1) do
      m_L.{i,j} <- compute_l i j
    done;
    v_D.{i} <- compute_d i
  done;
  m_L, v_D


let make_ldl ?target_rank m_A =
  full_ldl m_A



let test_case () =

  let matrix_diff m_A m_B = 
    Mat.syrk_trace (Mat.sub m_A m_B) 
  in

  let vector_diff v_A v_B = 
    let v = Vec.sub v_A v_B in
    dot v v
  in

  let test_full () =
    let m_A =
      Mat.of_array [| [|   4. ;  12. ; -16. |] ;
                      [|  12. ;  37. ; -43. |] ;
                      [| -16. ; -43. ;  98. |] |]
    in

    let m_L_ref = 
      Mat.of_array [| [|   1. ;   0. ;   0. |] ;
                      [|   3. ;   1. ;   0. |] ;
                      [|  -4. ;   5. ;   1. |] |]
    in

(*
    let m_Lt_ref = 
      Mat.transpose_copy m_L_ref
    in
    *)

    let v_D_ref = 
      Vec.of_array [| 4. ; 1. ; 9. |]
    in

    let m_L, v_D = full_ldl m_A in

    Alcotest.(check (float 1.e-15)) "full L" 0. (matrix_diff m_L m_L_ref);
    Alcotest.(check (float 1.e-15)) "full D" 0. (vector_diff v_D v_D_ref)
  in
  [
    "Full", `Quick, test_full;
  ]