(P : Solver_param) (A : Acyclic_t) : Solver =
struct

  open P
  include A

  let print_rotations angles =
    Array.iteri
      (fun k (i,j,d) ->
         let (dt,dp) = angles.(2*k), angles.(2*k+1) in
           Printf.printf "(%d,%d) rot (%f,%f)\n" i j dt dp) bones

  let elementary_move_precise constraints =
    let (m,b) = equations constraints in
    let a = Matrix.solve m b in
    let oldpos = Array.copy pos in
      print_rotations a ;
      for k = 0 to nb_bones - 1 do
        let (i,j,d) = bones.(k) in
        let (xi,yi,zi) = pos.(i) in
        let (xj,yj,zj) = pos.(j) in
        let old = angles.(k) in
        let (theta,phi) = (mod2pi (old.theta +. a.(2*k)),
                           mod2pi (old.phi +. a.(2*k+1))) in
        let (nxj,nyj,nzj) =
          (xi +. d*.(sin phi)*.(cos theta),
           yi +. d*.(sin phi)*.(sin theta),
           zi +. d*.(cos phi)) in

          pos.(j) <- (nxj,nyj,nzj) ;
          angles.(k) <- {theta=theta;phi=phi} ; 

          Printf.printf
            "dM_%d :\n\tEffectif :\t%f %f %f\n\tAnnonc\233 :\t%f %f %f\n%!"
            j (nxj-.xj) (nyj-.yj) (nzj-.zj)
            a.(2*nb_bones+3*j+0)
            a.(2*nb_bones+3*j+1)
            a.(2*nb_bones+3*j+2) ;

      done

  let elementary_move constraints =
    elementary_move_precise (one_of_three constraints)

  let goal = ref [||]
  let rem_steps = ref 0

  let goal_length g =
    let n = ref 0 in
    let s = ref 0. in
      Array.iter (function
                    | None -> ()
                    | Some a -> incr n ; s := !s+.a*.a) g ;
      !s/.(float !n)

  let get_pos_of_precise i =
    let j = i/3 in
    let c = i-3*j in
    let (x,y,z) = pos.(j) in
      match c with
        | 0 -> x
        | 1 -> y
        | 2 -> z
        | _ -> failwith "get_pos_of_precise"

  let begin_move_in constraints steps =
    goal := constraints ;
    rem_steps := (match steps with None -> 10 | Some i -> i) ;
    for i = 0 to (Array.length !goal)-1 do
      match !goal.(i) with
        | None -> ()
        | Some d -> !goal.(i) <- Some (d+.(get_pos_of_precise i))
    done

  let begin_move_precise ?steps c = begin_move_in c steps
  let begin_move ?steps c = begin_move_in (one_of_three c) steps

  

(** move () returns true if the move is finished. *)


  let move () =
    let move = Array.copy !goal in
      for i = 0 to (Array.length move)-1 do
        match move.(i) with
          | None -> ()
          | Some d -> move.(i) <- Some (d-.(get_pos_of_precise i))
      done ;
      let length = goal_length move in
        if length < epsilon || !rem_steps = 0 then
          true
        else
          let movee = Array.make (Array.length !goal) None in
            for i = 0 to (Array.length move)-1 do
              match move.(i) with
                | None -> ()
                | Some x ->
                    let xx = x*.epsilon/.length in
                      movee.(i) <- Some xx
            done ;
            try
              Printf.printf "===== Step %d ... \n" !rem_steps ;
              decr rem_steps ;
              elementary_move_precise movee ;
              false
            with
              | e ->
                  Printf.printf "*** %s ***\n%!"
                  (Printexc.to_string e) ;
                  true

end