open StdLabels

type t =
  { p: Gg.v2
  ; size : float
  ; angle: float
  }

let create x y =
  { p = Gg.V2.v x y
  ; size = 0.1
  ; angle = Gg.Float.pi_div_4
  }

let (+) p1 p2 =
  { p1 with p = Gg.V2.(+) p1.p p2 }

let get_coord { p; _ } = p

let get_coord'
  : t -> Gg.v2
  = fun t ->
    let open Gg.V2 in
    let trans = of_polar @@ v t.size t.angle in
    t.p + trans

let return_segment
  : Curves.Bezier.t -> Curves.Bezier.t list -> Curves.Bezier.t list
  = fun bezier beziers ->
    (* We gave the points in reverse order, so we have to revert the
       curve *)
    let bezier' = Curves.Bezier.reverse bezier in
    bezier'::beziers


let get_new_segment connexion0 p5 p4 p3 p2 p1 =
  let p5' = get_coord p5
  and p4' = get_coord p4
  and p3' = get_coord p3
  and p2' = get_coord p2
  and p1' = get_coord p1 in

  let points_to_link =
    [ p1'
    ; p2'
    ; p3'
    ; p4'
    ; p5' ] in
  Curves.Bspline.to_bezier ?connexion0 points_to_link

let add_point_in_path
  : float -> float -> t list -> Curves.Bezier.t list -> t list * Curves.Bezier.t list
  = fun x y path beziers ->
    let lastClick = create x y in
    let (let*) v f =
      match v with
      | Ok bezier ->
        if Array.length bezier > 0 then
          f (Array.get bezier 0)
        else
          lastClick::path, beziers
      | _ ->
        lastClick::path, beziers
    in

    let connexion0 = match beziers with
      | hd::_ -> Some hd.Curves.Bezier.p1
      | _ -> None in

    match path with
    | p4::p3::p2::p1::_ ->
      let* bezier = get_new_segment connexion0
          lastClick p4 p3 p2 p1 in
      (* We remove the last point and add the bezier curve in the list*)
      let firsts = lastClick::p4::p3::p2::[] in
      firsts, return_segment bezier beziers
    | _ ->
      lastClick::path, beziers