q2.ml

open Printf

let good_enough a b prec =
    ( (abs_float (a -. b)) -. (abs_float (b))) <= prec

(*c'est original mais ca marche pas des masses*)
let rec euler eps fa fb t1 t2 t =
    if (good_enough t1 t eps) && (good_enough t2 t eps) then
        let fat1 = fa t1 in
        let fbt1 = fb t1 in
        let fat2 = fa t2 in
        let fbt2 = fb t2 in
        let xt1 = Matrix.compute_x fat1 fbt1 (Lacaml_D.Mat.dim1 fat1) (Lacaml_D.Mat.dim2 fat1) in
        let xt2 = Matrix.compute_x fat2 fbt2 (Lacaml_D.Mat.dim1 fat2) (Lacaml_D.Mat.dim2 fat2) in
        let mat = Lacaml_D.Mat.add xt1 xt2 in
        let hi = Lacaml_D.Mat.make (Lacaml_D.Mat.dim1 mat) (Lacaml_D.Mat.dim1 mat) (1./.(abs_float (t1-.t2))) in
        Lacaml_D.Mat.mul mat hi

    else
        let new_t = (t1+.t2)/.2. in
        if new_t > t then
            euler eps fa fb t1 new_t t
        else
            euler eps fa fb new_t t2 t


let deriv fa fb t fda fdb =
    let a = fa t in
    let b = fb t in
    let aT = Matrix.transpose a in
    let da = fda t in
    let db = fdb t in
    let adT = Matrix.transpose a in
    let x = Matrix.compute_x a b (Lacaml_D.Mat.dim1 a) (Lacaml_D.Mat.dim2 a) in
    let left_member = Lacaml_D.gemm aT a in
    let adTb = Lacaml_D.gemm adT b in
    let aTdb = Lacaml_D.gemm aT db in
    let adTax = Lacaml_D.gemm (Lacaml_D.gemm adT a) x in
    let aTdax = Lacaml_D.gemm (Lacaml_D.gemm aT da) x in
    let right_member = Lacaml_D.Mat.sub (Lacaml_D.Mat.sub (Lacaml_D.Mat.add adTb aTdb) adTax) aTdax in
    Lacaml_D.gesv left_member right_member;
    right_member

let print a =
    printf "%f\n" a