(* Approximation of zero *)                
let leps l = (abs_float l) < epsilon_float

type point = float array

type vector = float array

(* vector addition *)
let ( |+| ) a b =
        [| a.(0) +. b.(0); a.(1) +. b.(1); a.(2) +. b.(2) |] 

(* vector subtraction *)
let (|-|) a b =
        [| a.(0) -. b.(0); a.(1) -. b.(1); a.(2) -. b.(2) |] 

(* scalar-vector multiplication*)
let (|*|) k a =
        [| k *. a.(0); k *. a.(1); k *. a.(2) |] 

(* dot product *)
let (|**|) a b =
        a.(0) *. b.(0) +. a.(1) *. b.(1) +. a.(2) *. b.(2)
        
(* cross product *)        
let (|^|) v1 v2 = 
        [| v1.(1)*.v2.(2) -. v2.(1)*.v1.(2);  v1.(2)*.v2.(0) -. v2.(2)*.v1.(0);  v1.(0)*.v2.(1) -. v2.(0)*.v1.(1) |]

(* length of a vector *)
let vlength a =
        sqrt (a |**| a)

(* normalize vector *)
let vnorm a = 
        (1.0 /. (vlength a)) |*| a                

type state = { stpos: point; stvel: vector; staccel: vector}

type derivative = { dstpos: point; dstvel: vector }

let euler st dt =
        {
                stpos = st.stpos |+| (dt |*| st.stvel);
                stvel = st.stvel |+| (dt |*| st.staccel);
                staccel = st.staccel
        }

let newton_stormer_verlet st dt =
        let v = st.stvel |+|  (dt |*| st.staccel) in
        {
                stpos = st.stpos |+|  (dt |*| v);
                stvel = v;
                staccel = st.staccel
        }

let runge_kutta_4th_order st dt =
        let evaluate dt d =
                let s = { 
                                stpos = st.stpos |+| (dt |*| d.dstpos);
                                stvel = st.stvel |+| (dt |*| d.dstvel);
                                staccel = st.staccel 
                        } 
                in
                {
                        dstpos = s.stvel;
                        dstvel = s.staccel
                }
        in          
        let a = evaluate 0.0 { dstpos = st.stvel; dstvel = st.staccel} in
        let b = evaluate (dt *. 0.5) a in
        let c = evaluate (dt *. 0.5) b in
        let d = evaluate dt c in
        let dxdt = 1.0 /. 6.0 |*| (a.dstpos |+| (2.0 |*| (b.dstpos |+| c.dstpos)) |+| d.dstpos) 
        and dvdt = 1.0 /. 6.0 |*| (a.dstvel |+| (2.0 |*| (b.dstvel |+| c.dstvel)) |+| d.dstvel) in
        {
                stpos = st.stpos |+| (dt |*| dxdt);
                stvel = st.stvel |+| (dt |*| dvdt);
                staccel = st.staccel
        }

let explicit_verlet st dt =
        let x0 = st.stpos |-| (dt |*| st.stvel) and x1 = st.stpos in
        let v = x1 |-| x0 |+| ((dt *. dt) |*| st.staccel) in
        {
                stpos = x1 |+| v;
                stvel = (1.0 /. dt) |*| v;
                staccel = st.staccel
        }

let evaluate integrator initial time dt = 
        let rec eval st t = 
                if t >= time then st else
                eval (integrator st dt) (t +. dt)
        in
        eval initial 0. 
        

(* Constraints *)

type path_constraint = 
        {
                get_nearest_valid_point : point -> point;
                get_tangent_velocity : point -> vector -> vector
        }

(* Bead on wire hoop *)

type hoop = { center : point; radius : float}

let bead1 = { stpos = [|100.; 0.; 0.;|]; stvel = [|0.; 30.; 0.;|]; staccel = [|-40.; -10.; 0.|] }

let print_bead p t x y = 
        let s = Printf.sprintf "%s speed= %f" t (vlength p.stvel) in
        Graphics.moveto x y;
        Graphics.draw_string s 
        
let make_hoop center radius = 
        let get_pt pt = 
                let l = pt |-| center in
                let d = vlength l in
                center |+| ((radius /. d) |*| l )
        and get_tan pt v =
                let speed = vlength v in
                if (leps speed) then
                        v
                else
                        let vel = vnorm v in
                        let l = vnorm (pt |-| center) in
                        let parallel = (vel |**| l) |*| l in
                        let perpendicular = vel |-| parallel in
                        (speed) |*| (vnorm perpendicular)
        in
        {
                get_nearest_valid_point = get_pt;
                get_tangent_velocity = get_tan
        }        

let update_system hp bead time dt =
        let bead' = evaluate euler bead time dt in
        let x0 = bead.stpos and x0' = bead'.stpos in
        let x1 = hp.get_nearest_valid_point x0' in
        let v0 = bead.stvel and v0' = bead'.stvel in
        let v1 = hp.get_tangent_velocity x1 v0' in
        let result = {stpos = x1; stvel = v1; staccel = bead.staccel} in
        print_bead result "bead: " 10 10;
        result


(* Test *)
let width = 500
let height = 500

let center =  [|0.; 0.; 0.;|]
let radius = 100.0

let hoop1  = make_hoop center radius

let draw_bead bead = 
        Graphics.set_color Graphics.black;
        let p = bead.stpos and i = truncate in
        Graphics.fill_circle ((i p.(0)) + width/2) ((i p.(1)) + height/2) 3

let draw_hoop c r =
        let i = truncate in
        Graphics.draw_circle ((i c.(0)) + width/2) ((i c.(1)) + height/2) (i r)

let init_graphics () =
        let init_string = (Printf.sprintf " %dx%d" width height) in
        Graphics.open_graph init_string;
        Graphics.set_window_title "Ball on bead demo"
        

let rec main t0 st0 =
        let _ = Graphics.auto_synchronize false in
        let _ = Graphics.clear_graph () in 
        let t1 = Unix.gettimeofday () in 
        let dt = t1 -. t0 in
        let st1 = update_system hoop1 st0 dt (dt /. 100.0) in
        draw_hoop center radius;
        draw_bead st1;
        let _ = Graphics.auto_synchronize true in
        let status = Graphics.wait_next_event [Graphics.Poll; Graphics.Key_pressed] in
        if (status.Graphics.keypressed = false) then main t1 st1 

let ()  = init_graphics (); main (Unix.gettimeofday ()) bead1