(P : Param) : Acyclic_t =
struct

  let skel = P.skel
  let n = Array.length skel
  let pos = Array.copy P.pos

  let nb_bones =
    let n = ref 0 in
      Array.iter
        (fun line ->
           Array.iter (fun e -> if e then incr n) line)
        skel ;
      assert ((!n/2)*2 = !n) ;
      !n/2

  let root = ref (-1)

  

(** The directed bones (source, destination, length). *)


  let bones = Array.create nb_bones (0,0,0.) 
      
  

(** Angles (orientation) of the bones. *)


  let angles = Array.create nb_bones { theta=0. ; phi=0. }

  let do_theta x y =
    let a = atan (y/.x) in
      if x > 0. then a else a+.pi
 
  let update_angles () =
    let do_angles index bone =
      let (i,j,d) = bone in
      let (xi,yi,zi) = pos.(i) in
      let (xj,yj,zj) = pos.(j) in
      let (x,y,z) = (xj-.xi, yj-.yi, zj-.zi) in
      let newangles =
        { theta =
            if x = 0. then
              if y>=0. then halfpi else mhalfpi
            else
              do_theta x y ;
          phi =
            acos (z/.d) } in
        Printf.printf "Angle %d (%f,%f,%f) : (%f,%f)\n%!"
          index x y z
          newangles.theta newangles.phi ;
        angles.(index) <- newangles
    in
      Array.iteri do_angles bones

  

(** Parse the skel, starting from the root, in order to get the new bones, topologicaly sorted. *)


  let init r =
    (* We'll width-first parse the skel,
     * starting from the root,
     * in order to get the new bones, topologicaly sorted. *)
    let index = ref 0 in
    let add_bones i father =
      let xi,yi,zi = pos.(i) in
        Array.iteri
          (fun j connected ->
             if connected && j <> father then
               let xj,yj,zj = pos.(j) in
               let x,y,z = xj-.xi,yj-.yi,zj-.zi in
               let d = sqrt (x*.x+.y*.y+.z*.z) in
                 bones.(!index) <- (i,j,d) ;
                 incr index ;
                 Printf.printf "Bone (%d,%d,%f)\n" i j d) skel.(i)
    in
    let rec round backup =
      let new_backup = !index in
        for i = backup to !index - 1 do
          let (a,b,_) = bones.(i) in
            try
              add_bones b a
            with
              | Invalid_argument ("index out of bounds") ->
                  failwith "Skel is cyclic!"
        done ;
        if new_backup <> !index then
          round new_backup
    in
      if !root <> r then
        begin
          root := r ;
          add_bones r (-1) ;
          round 0 ;
          if !index <> nb_bones then
            failwith "Skel is not connex!" ;
          update_angles () ;
          Printf.printf "Root changed to %d.\n" !root
        end
        
  let () = init P.root

  (*
    Spherical to cartesian
    ---------------------------------------------------------------------------
    x = r sin(phi) cos(theta)
    y = r sin(phi) sin(theta)
    z = r cos(phi)
    
    So the variation is (r remaining constant):
    dx = r (dphi cos(theta) cos(phi) - dtheta sin(phi) sin(theta))
    dy = r (dphi sin(theta) cos(phi) + dtheta sin(phi) cos(theta))
    dz = - r dphi sin(phi)                                                   *)

  (*
    Encoding the state of the skel,
    where b is the number of bones, p the number of points.
    --------------------------------------------------------------------------
    2*b floats : dtheta_1, dphi_1, dtheta_2, dphi_2 ... dtheta_b, dphi_b
    3*p floats : dx_1, dy_1, dz_1 ...
    So we have a l=(2*b+3*p) vector.
    The equations will be of two sorts.
    1) m moving constraints
    2) 3*b skeleton constraints : the bones's constraints (one per axis)     *)

  let equations move =
    move.(3* !root+0) <- Some 0. ;
    move.(3* !root+1) <- Some 0. ;
    move.(3* !root+2) <- Some 0. ;
    let m =
      let m = ref 0 in
        Array.iter (fun a -> if a <> None then incr m) move ;
        !m
    in
    let p = Array.length pos in
    let b = nb_bones in
    let l = 2*b+3*p in (* vector's length *)
    let eq = Array.make (m+3*b) [||] in
    let cst = Array.make (m+3*b) 0. in
    let eq_i = ref 0 in

      (* Moving constraints *)
      Array.iteri
        (fun i -> function
           | None -> ()
           | Some d ->
               let e = Array.make l 0. in
                 e.(2*b+i) <- 1. ;
                 eq.(!eq_i) <- e ; cst.(!eq_i) <- d ; incr eq_i )
        move ;

      (* Skeleton constraints *)
      for k = 0 to b-1 do
        let (i,j,d) = bones.(k) in
        let {theta=theta;phi=phi} = angles.(k) in
        let ex = Array.make l 0. in
        let ey = Array.make l 0. in
        let ez = Array.make l 0. in
        let o = 2*k in

          (* Here we encode
           * dx_j = dx_i - dtheta sin phi sin theta + dphi cos phi cos theta *)
          ex.(o+0) <- -. d *. (sin phi) *. (sin theta) ;
          ex.(o+1) <-    d *. (cos phi) *. (cos theta) ;
          ex.(2*b+3*j+0) <- -1. ;
          ex.(2*b+3*i+0) <- 1. ;
          eq.(!eq_i) <- ex ; incr eq_i ;

          (* Similar stuff for dy ... *)
          ey.(o+0) <- d *. (sin phi) *. (cos theta) ;
          ey.(o+1) <- d *. (cos phi) *. (sin theta) ;
          ey.(2*b+3*j+1) <- -1. ;
          ey.(2*b+3*i+1) <- 1. ;
          eq.(!eq_i) <- ey ; incr eq_i ;

          (* ... and dz. *)
          ez.(o+1) <- -. d *. (sin phi) ;
          ez.(2*b+3*j+2) <- -1. ;
          ez.(2*b+3*i+2) <- 1. ;
          eq.(!eq_i) <- ez ; incr eq_i ;

      done ;

      assert (!eq_i = m+3*b) ;
      (eq, cst)
end