open StdLabels

type err = [`InvalidPath ]

module M = Matrix.MakeMatrix (struct

    type t = Float.t

    let compare a b =

      let v = Float.compare a b in
      if v = 0 then Matrix.Order.Equal
      else if v > 0 then Matrix.Order.Greater
      else Matrix.Order.Less

    let zero = Float.zero
    let one = Float.one

    let divide = (/.)
    let multiply = ( *. )
    let add = (+.)
    let subtract = (-.)
    exception NonElt


  end)

type t = Gg.v2 list

let from_points
  : Gg.v2 array -> (Gg.v2 array, [> `InvalidPath]) Result.t
  = fun points ->

    let n = (Array.length points - 2) in

    if n <= 1 then
      Result.error `InvalidPath
    else

      (* Create the initial matrix.

         The matrix is augmented with two additionals columns, which will be
         populated with the points from the path.
      *)
      let arr = Array.init n ~f:(fun line ->
          Array.init (n +2) ~f:(fun row ->
              match row - line with
              | (-1) -> 1.
              | 0 -> 4.
              | 1 -> 1.
              | _ -> 0.
            )
        ) in
      let matrix = M.from_array arr in

      (* Add the points from the augmented matrix *)
      let points_array = points in
      for line = 0 to (n -1) do

        let point =
          if line = 0 then
            let p0 = points_array.(0)
            and p1 = points_array.(1) in
            Gg.V2.(6. * p1 - p0)
          else if (line + 1) = n  then
            let p_n_2 = points_array.(n)
            and p_n_1 = points_array.(n + 1) in
            Gg.V2.(6. * p_n_2 - p_n_1)
          else
            let n' = line + 1 in
            Gg.V2.(6. * points_array.(n'))
        in
        let x = (Gg.V2.x point)
        and y = (Gg.V2.y point) in


        M.set_elt matrix (line + 1, n + 1) x;
        M.set_elt matrix (line + 1, n + 2) y;
      done;

      (* Resolve the matrix *)
      let res' = M.row_reduce matrix in

      (* Extract the result as points *)
      let _, col_x = M.get_column res' (n + 1)
      and _, col_y = M.get_column res' (n + 2) in

      (* Build the result *)
      let res = Array.make (n + 2) (Array.get points_array (n + 1) ) in
      for i = 1 to n do
        let point = Gg.V2.v col_x.(i - 1) col_y.(i - 1) in
        Array.set res i point;
      done;
      Array.set res 0 (Array.get points_array 0);
      Result.ok res

let (let*) = Result.bind

(** Build a continue curve from path

    see https://www.math.ucla.edu/~baker/149.1.02w/handouts/dd_splines.pdf
*)
let to_bezier
  : ?connexion0:Gg.v2 -> ?connexion1:Gg.v2 -> t -> (Bezier.t array, [> `InvalidPath]) Result.t
  = fun ?connexion0 ?connexion1 points ->

    let points' = match connexion0 with
      | None -> points
      | Some pt -> pt::points in

    let arr_points = match connexion1 with
      | None -> Array.of_list points'
      | Some pt ->
        let arr = Array.make (1 + (List.length points')) pt in
        List.iteri points'
          ~f:(fun i value -> Array.set arr i value);
        arr in

    let* bspline_points = from_points arr_points in

    let start = match connexion0 with
      | None -> 1
      | Some _ -> 2
    and end_ = match connexion1 with
      | None -> (Array.length bspline_points) - 1
      | Some _ -> (Array.length bspline_points) - 2 in

    let result = Array.init (end_ - start + 1) ~f:(fun i ->

        let i = i + start in

        let prev_b = Array.get bspline_points (i - 1)
        and bpoint = Array.get bspline_points i
        and prev_p = Array.get arr_points (i - 1)
        and point = Array.get arr_points i in
        let ctrl0 = Gg.V2.(mix prev_b bpoint (1. /. 3.))
        and ctrl1 = Gg.V2.(mix prev_b bpoint (2. /. 3.)) in

        let bezier =
          { Bezier.p0 = prev_p
          ; Bezier.p1 = point
          ; Bezier.ctrl0
          ; Bezier.ctrl1 } in

        bezier

      ) in
    Result.Ok result