open Jog open Zone;;
let coo = [|[|0.; 0.; 1.|];
[|0.; 1.; 0.|]; [|1.; 0.; 0.|]; [|0.; -1.; 0.|]; [|-1.; 0.; 0.|];
[|0.; 0.; 0.|]|]
and sq w = w *. w in
let pyt i j = let s = ref 0. in for k=0 to (len coo.(0)) - 1 do s := !s +.
     (sq (coo.(i).(k) -. coo.(j).(k))) done; !s
and m i = 1<i && i< 4 in
let top = [[5; 0; 1; 2]; [5; 0; 2; 3]; [5; 0; 3; 4]; [5; 0; 4; 1]]
and ini = ref (fun _ -> raise (OffTrack 3))
and ds2 i j = if (m i) && (m j) && i <> j then 2. else pyt i j
and fac = [0; 1; 5] and bid = 1 in
let fa = Array.of_list fac in
let n = len fa in
let gd = aini n (fun i -> aini n (fun j -> 0.5 *. (
  (ds2 fa.(n-1) fa.(i)) +. (ds2 fa.(n-1) fa.(j)) -. (ds2 fa.(i) fa.(j))))) in
  for j = 0 to n-2 do gd.(j).(n-1) <- 0.; gd.(n-1).(j) <- 0. done; gd.(n-1).(n-1) <- 1.;
let ip a b = let s = ref 0. in for i=0 to n-1 do for j=0 to n-1 do
   s := !s +. a.(i) *. b.(j) *. gd.(i).(j) done done; !s in
let pON = Linear.gs ip (let fON = Linear.iden n in
     (let t = fON.(bid) in fON.(bid) <- fON.(n-2); fON.(n-2) <- t); fON) in pzm "pON" pON;
let _ = Glue.morphgen (nz ds2) top
    [(fac, ((fun  x -> x),
      (fun t -> ini := t.curl; {t with curl = (fun (k, ball) -> ball)})))] in
    (pON, Linear.mul (Linear.inv pON) (zzm "u" (!ini (bid, pON))))

(* should yield (_, iden 3) *);;