type vecteur = {x:float ; y:float};;

(* let N = 3;;
let univers = [|{mass = 1 ; pos = vecteur_nul ; vel = vecteur_nul ; acc = vecteur_nul};
                {mass = 2 ; pos = vecteur_nul ; vel = vecteur_nul ; acc = vecteur_nul};
                {mass = 3 ; pos = vecteur_nul ; vel = vecteur_nul ; acc = vecteur_nul}|];;
let taille_univers = 1997;; *)

let vecteur_nul = {x = 0.0 ; y = 0.0};;

type corps = {mass:float ; mutable pos:vecteur ; mutable vel:vecteur ; mutable acc:vecteur};;

let add u v = {x = u.x +. v.x ; y = u.y +. v.y}
and sub u v = {x = u.x -. v.x ; y = u.y -. v.y};;

let scal m u = {x = m *. u.x ; y = m *. u.y};;

let carre u = u.x *. u.x +. u.y *. u.y;;

type arbre = Noeud of cellule |Feuille of corps |Vide
and cellule =
  {mutable cm_mass:float;   (* masse des corps de la cellule *)
   mutable cm_pos: vecteur; (* centre de masse des corps de la cellule *)
   filles:arbre vect        (* 4 sous-arbres *)
  };;
  
let indice_fille p_c p =
  let v = sub p p_c
  in
  if v.x >= 0. then if v.y >= 0. then 0 else 1
  else if v.y <= 0. then 2 else 3;;

let position_fille p_c taille i =
  let u = {x = 1. ; y = 1.} and v = {x = 1. ; y = -1.} and m = taille /. 2.
  in
  match i with
     0 -> add p_c (scal m u)
    |1 -> add p_c (scal m v)
    |2 -> sub p_c (scal m u)
    |_ -> sub p_c (scal m v);;
    
let rec insere_corps c q position côté =
  match q with
     Vide -> Feuille c
    |Noeud cellule ->
      let i = indice_fille position c.pos
      in insere_corps c cellule.filles.(i) (position_fille position côté i) (côté /. 2.)
    |Feuille c' ->
      let nouveau = Noeud {cm_mass = 0. ; cm_pos = vecteur_nul ; filles = make_vect 4 Vide}
      in insere_corps c (insere_corps c' nouveau position côté) position côté;;
      
let rec barycentres = function
   Noeud cell -> for i = 0 to 3
                 do
                   let f = cell.filles.(i)
                   in
                   match f with
                      Noeud cl  ->barycentres f;
                                  cell.cm_mass <- cell.cm_mass +. cl.cm_mass;
                                  cell.cm_pos <- add cell.cm_pos (scal cl.cm_mass cl.cm_pos)
                     |Feuille c ->cell.cm_mass <- cell.cm_mass +. c.mass;
                                  cell.cm_pos <- add cell.cm_pos (scal c.mass c.pos)
                     |Vide      ->()
                 done;
                 cell.cm_pos <- scal (1. /. cell.cm_mass) cell.cm_pos
  |_          -> ();;
  
let theta = 1.;;

let grav_approx pos arbre taille =
  let acc_approx = ref vecteur_nul and forêt = ref [arbre] and côté = ref taille
  in
  let rec traite f t =
    match f with
       [] -> []
      |Vide::q -> traite q t 
      |(Feuille c)::q ->  
              let r = sub c.pos pos
              in
              if r <> vecteur_nul then
                let d = sqrt (carre r)
                in acc_approx := add !acc_approx (scal (c.mass /. (d *. d *. d)) r)
              else ();
              traite q t
      |(Noeud cell)::q ->
              let r = sub cell.cm_pos pos
              in
              let d = sqrt (carre r)
              in             
              if t < d *. theta then
                (acc_approx := add !acc_approx (scal (cell.cm_mass /. (d *. d *. d)) r);
                traite q t)
              else
                cell.filles.(0)::cell.filles.(1)::cell.filles.(2)::cell.filles.(3)::(traite q t)
  in
  while !forêt <> [] do forêt := traite !forêt !côté; côté := !côté /. 2. done;
  !acc_approx;;
   
let rec grav_approx pos arbre taille =
  match arbre with
     Vide -> vecteur_nul
    |Feuille c ->
      let r = sub c.pos pos
      in
      if r = vecteur_nul then vecteur_nul
      else let d = sqrt (carre r) in scal (c.mass /. (d *. d *. d)) r
    |Noeud cell ->
      if taille < (sqrt (carre (sub cell.cm_pos pos))) *. theta then
        let c' = {mass = cell.cm_mass ; pos = cell.cm_pos ; vel = vecteur_nul ; acc = vecteur_nul}
        in grav_approx pos (Feuille c') taille
      else
        begin
          let acc_approx = ref vecteur_nul
          in
          for k = 0 to 3
          do
            acc_approx := add !acc_approx (grav_approx pos cell.filles.(k) (taille /. 2.))
          done;
          !acc_approx
        end;;