open Graphics ;;

Random.self_init () ;;

let __width__ = 1500
and __height__ = 1000 ;;

(*

ocamlfind ocamlc -linkpkg -package unix -linkpkg -package graphics -thread -package threads -linkpkg display.ml

*)

(* ------------------------------------------------------------- *)
(* ------------------------------------------------------------- *)

type 'a dynamic = {mutable tab : 'a array ; mutable len : int ; mutable memlen : int} ;;

let dyn_create i =
  {tab = Array.make 10 i ; len = 0 ; memlen = 10} ;;

let dyn_append arr elt =
  let fct x =
    if x < arr.len then
      arr.tab.(x)
    else
      arr.tab.(0)
  in
  if arr.len = arr.memlen then begin
    let newarr = Array.init (2 * arr.memlen) fct in
    arr.memlen <- 2 * arr.memlen;
    arr.tab <- newarr
  end;
  arr.tab.(arr.len) <- elt;
  arr.len <- arr.len + 1 ;;

(* ------------------------------------------------------------- *)
(* ------------------------------------------------------------- *)

let matrix_mult m1 m2 =
  let n = Array.length m1
  and p = Array.length m1.(0)
  and r = Array.length m2
  and s = Array.length m2.(0) in

  let mres = Array.make_matrix n s 0 in

  if p <> r then
    failwith "ERROR : matrixes cannot be multipied, maybe try with reversed inputs ?\n"
  else begin
    for i = 0 to n-1 do
      for j = 0 to s-1 do
        for k = 0 to p do
          mres.(i).(j) <- mres.(i).(j) + m1.(i).(k) + m2.(k).(j)
        done
      done
    done
  end;
  mres ;;

let abs x = if x >= 0 then x else -x ;;
let absf x = if x >= 0. then x else -.(x) ;;

(* ------------------------------------------------------------- *)

type pt_3d = {mutable x : float ; mutable y : float ; mutable z : float} ;;
type pt_2d = {mutable x : float ; mutable y : float} ;;

(* ------------------------------------------------------------- *)
let camera_xyz = {x = 0.0 ; y = 0.0 ; z = 0.0} ;;
let camera_angle_x = ref 0 ;;
let camera_angle_y = ref 0 ;;
let camera_angle_z = ref 0 ;;
(* in degrees *)
(* ------------------------------------------------------------- *)

(*
let should_be_drawn (pt : pt_3d) =
  let translated = {x = pt.x -. camera_xyz.x; y = pt.y -. camera_xyz.y; z = pt.z +. camera_xyz.z} in
  (translated.z *. Float.cos ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.) -. translated.x *. Float.sin ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.)) > 0. ;;
*)

let should_be_drawn_gr (pt : pt_3d) =
  pt.z > 0.1 ;;

let sign x =
  if x >= 0. then 1. else -. (1.) ;;

let is_cube_behind_camera (cube : pt_3d array) = 
  let res = ref true in
  for i = 0 to (Array.length cube) -1 do
    ()
  done;
  !res ;;

let debug_1 (smth : pt_3d array) =
  for i = 0 to Array.length smth -1 do
    Printf.printf "(%f, %f, %f)" smth.(i).x smth.(i).y smth.(i).z;
    Stdlib.print_endline ";"
  done ;
  Stdlib.print_endline " " ;;

let to_graphics (flat : pt_2d array) screen_wd screen_ht =
  let res = Array.make (Array.length flat) (0, 0) in
  for k = 0 to (Array.length flat -1) do
    let proj_x = int_of_float ((float_of_int screen_wd) *. (1. +. flat.(k).x) /. 2.)
    and proj_y = int_of_float ((float_of_int screen_ht) *. (1. +. flat.(k).y) /. 2.) in
    (*Printf.printf "Converting to (%d %d)" proj_x proj_y;
    Stdlib.print_endline " ";*)
    res.(k) <- (proj_x, proj_y);
  done;
  res ;;

let draw_pts_2d (flat : pt_2d array) screen_wd screen_ht =
  set_color black;
  set_line_width 4;
  for k = 0 to (Array.length flat -1) do
    if absf flat.(k).x <= 1.01 && absf flat.(k).y <= 1.01 then begin
      let proj_x = int_of_float ((float_of_int screen_wd) *. (1. +. flat.(k).x) /. 2.)
      and proj_y = int_of_float ((float_of_int screen_ht) *. (1. +. flat.(k).y) /. 2.) in
      (*Printf.printf "Printing at (%d %d)" proj_x proj_y;
      Stdlib.print_endline " ";*)
      fill_circle proj_x proj_y 10
    end
  done ;;

let project (shape : pt_3d array) screen_wd screen_ht fov = 
  let res = Array.make (Array.length shape) {x = 0. ; y = 0.} in 
  for k = 0 to (Array.length shape -1) do
    res.(k) <- {x = 2. ; y = 2.}
  done;
  let ar = (float_of_int screen_wd) /. (float_of_int screen_ht) in
  for k = 0 to (Array.length shape -1) do
    if should_be_drawn_gr shape.(k) then begin
      res.(k).x <- shape.(k).x /. (ar *. shape.(k).z *. Float.tan (((float_of_int fov) *. 3.14159265358 /. 180.) /. 2.));
      res.(k).y <- shape.(k).y /. (shape.(k).z *. Float.tan (((float_of_int fov) *. 3.14159265358 /. 180.) /. 2.))
    end
    else begin
      res.(k).x <- (absf shape.(k).x) /. (ar *. (0.1 *. (sign shape.(k).x)) *. Float.tan (((float_of_int fov) *. 3.14159265358 /. 180.) /. 2.));
      res.(k).y <- (absf shape.(k).y) /. ((0.1 *. (sign shape.(k).y)) *. Float.tan (((float_of_int fov) *. 3.14159265358 /. 180.) /. 2.))
    end;
    (*Printf.printf "added (%f %f)" res.(k).x res.(k).y;
    Stdlib.print_endline " ";*)
  done;
  res ;;

let adjust_to_camera (shape : pt_3d array) =
  let res = Array.make (Array.length shape) {x = 0.0 ; y = 0.0; z = 0.0} in
  for i = 0 to Array.length shape -1 do
    res.(i) <- {x = shape.(i).x +. camera_xyz.x ; y = shape.(i).y +. camera_xyz.y ; z = shape.(i).z -. camera_xyz.z}
  done;
  let res2 = Array.make (Array.length shape) {z =0.0 ; x =0.0 ; y =0.0} in
  for i = 0 to Array.length shape -1 do
    res2.(i) <- {
      x = res.(i).x *. Float.cos ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.) +. res.(i).z *. Float.sin ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.);
      y = res.(i).y;
      z = res.(i).z *. Float.cos ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.) -. res.(i).x *. Float.sin ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.)
    }
  done;
  (*debug_1 res2 ;*)
  res2 ;;

let sq x = x *. x ;;

let dist_from_camera (p : pt_3d) =
  Float.sqrt ((sq (p.x +. camera_xyz.x)) +. (sq (p.y +. camera_xyz.y)) +. (sq (p.z -. camera_xyz.z))) ;;

let farthest_pt (p1 : pt_3d) (p2 : pt_3d) =
  max (dist_from_camera p1) (dist_from_camera p2) ;;

let swap arr i j =
  let temp = arr.(i) in
  arr.(i) <- arr.(j);
  arr.(j) <- temp ;;

let are_faces_behind (cube : pt_3d array) = 
  let res = Array.make 6 false in
  res.(0) <- (should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(2)) || (should_be_drawn_gr cube.(3));
  res.(1) <- (should_be_drawn_gr cube.(4)) || (should_be_drawn_gr cube.(5)) || (should_be_drawn_gr cube.(6)) || (should_be_drawn_gr cube.(7));
  res.(2) <- (should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(5)) || (should_be_drawn_gr cube.(4));
  res.(3) <- (should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(2)) || (should_be_drawn_gr cube.(6)) || (should_be_drawn_gr cube.(5));
  res.(4) <- (should_be_drawn_gr cube.(2)) || (should_be_drawn_gr cube.(3)) || (should_be_drawn_gr cube.(7)) || (should_be_drawn_gr cube.(6));
  res.(5) <- (should_be_drawn_gr cube.(3)) || (should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(4)) || (should_be_drawn_gr cube.(7));
  (res, res.(0) || res.(1) || res.(2) || res.(3) || res.(4) || res.(5)) ;;

let are_faces_behind_rev (cube : pt_3d array) = 
  let res = Array.make 6 false in
  res.(0) <-
  ((should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(2))) &&
  ((should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(3))) && 
    ((should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(1))) &&
    ((should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(2))) &&
    ((should_be_drawn_gr cube.(2)) || (should_be_drawn_gr cube.(3))) &&
    ((should_be_drawn_gr cube.(3)) || (should_be_drawn_gr cube.(0))) ;
  res.(1) <- 
  ((should_be_drawn_gr cube.(4)) || (should_be_drawn_gr cube.(6))) &&
  ((should_be_drawn_gr cube.(5)) || (should_be_drawn_gr cube.(7))) &&
    ((should_be_drawn_gr cube.(4)) || (should_be_drawn_gr cube.(5))) &&
    ((should_be_drawn_gr cube.(5)) || (should_be_drawn_gr cube.(6))) &&
    ((should_be_drawn_gr cube.(6)) || (should_be_drawn_gr cube.(7))) &&
    ((should_be_drawn_gr cube.(7)) || (should_be_drawn_gr cube.(4))) ;
  res.(2) <- 
  ((should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(5))) &&
  ((should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(4))) &&
    ((should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(1))) &&
    ((should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(5))) &&
    ((should_be_drawn_gr cube.(5)) || (should_be_drawn_gr cube.(4))) &&
    ((should_be_drawn_gr cube.(4)) || (should_be_drawn_gr cube.(0))) ;
  res.(3) <- 
  ((should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(6))) &&
  ((should_be_drawn_gr cube.(2)) || (should_be_drawn_gr cube.(5))) &&
    ((should_be_drawn_gr cube.(1)) || (should_be_drawn_gr cube.(2))) &&
    ((should_be_drawn_gr cube.(2)) || (should_be_drawn_gr cube.(6))) &&
    ((should_be_drawn_gr cube.(6)) || (should_be_drawn_gr cube.(5))) &&
    ((should_be_drawn_gr cube.(5)) || (should_be_drawn_gr cube.(1))) ;
  res.(4) <- 
  ((should_be_drawn_gr cube.(2)) || (should_be_drawn_gr cube.(7))) &&
  ((should_be_drawn_gr cube.(3)) || (should_be_drawn_gr cube.(6))) &&
    ((should_be_drawn_gr cube.(2)) || (should_be_drawn_gr cube.(3))) &&
    ((should_be_drawn_gr cube.(3)) || (should_be_drawn_gr cube.(7))) &&
    ((should_be_drawn_gr cube.(7)) || (should_be_drawn_gr cube.(6))) &&
    ((should_be_drawn_gr cube.(6)) || (should_be_drawn_gr cube.(2))) ;
  res.(5) <- 
  ((should_be_drawn_gr cube.(3)) || (should_be_drawn_gr cube.(4))) &&
  ((should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(7))) &&
    ((should_be_drawn_gr cube.(3)) || (should_be_drawn_gr cube.(0))) &&
    ((should_be_drawn_gr cube.(0)) || (should_be_drawn_gr cube.(4))) &&
    ((should_be_drawn_gr cube.(4)) || (should_be_drawn_gr cube.(7))) &&
    ((should_be_drawn_gr cube.(7)) || (should_be_drawn_gr cube.(3))) ;
    (res, res.(0) || res.(1) || res.(2) || res.(3) || res.(4) || res.(5)) ;;

let are_edges_behind (cube : pt_3d array) = 
  let res = Array.make 6 [||] in
  res.(0) <- [|
    (should_be_drawn_gr cube.(0)) && (should_be_drawn_gr cube.(1));
    (should_be_drawn_gr cube.(1)) && (should_be_drawn_gr cube.(2));
    (should_be_drawn_gr cube.(2)) && (should_be_drawn_gr cube.(3));
    (should_be_drawn_gr cube.(3)) && (should_be_drawn_gr cube.(0))
  |];
  res.(1) <- [|
    (should_be_drawn_gr cube.(4)) && (should_be_drawn_gr cube.(5));
    (should_be_drawn_gr cube.(5)) && (should_be_drawn_gr cube.(6));
    (should_be_drawn_gr cube.(6)) && (should_be_drawn_gr cube.(7));
    (should_be_drawn_gr cube.(7)) && (should_be_drawn_gr cube.(4))
  |];
  res.(2) <- [|
    (should_be_drawn_gr cube.(0)) && (should_be_drawn_gr cube.(1));
    (should_be_drawn_gr cube.(1)) && (should_be_drawn_gr cube.(5));
    (should_be_drawn_gr cube.(5)) && (should_be_drawn_gr cube.(4));
    (should_be_drawn_gr cube.(4)) && (should_be_drawn_gr cube.(0))
  |];
  res.(3) <- [|
    (should_be_drawn_gr cube.(1)) && (should_be_drawn_gr cube.(2));
    (should_be_drawn_gr cube.(2)) && (should_be_drawn_gr cube.(6));
    (should_be_drawn_gr cube.(6)) && (should_be_drawn_gr cube.(5));
    (should_be_drawn_gr cube.(5)) && (should_be_drawn_gr cube.(1))
  |];
  res.(4) <- [|
    (should_be_drawn_gr cube.(2)) && (should_be_drawn_gr cube.(3));
    (should_be_drawn_gr cube.(3)) && (should_be_drawn_gr cube.(7));
    (should_be_drawn_gr cube.(7)) && (should_be_drawn_gr cube.(6));
    (should_be_drawn_gr cube.(6)) && (should_be_drawn_gr cube.(2))
  |];
  res.(5) <- [|
    (should_be_drawn_gr cube.(3)) && (should_be_drawn_gr cube.(0));
    (should_be_drawn_gr cube.(0)) && (should_be_drawn_gr cube.(4));
    (should_be_drawn_gr cube.(4)) && (should_be_drawn_gr cube.(7));
    (should_be_drawn_gr cube.(7)) && (should_be_drawn_gr cube.(3))
  |];
  res ;;

let draw_cube_p (cube : pt_3d array) screen_wd screen_ht fov r g b =
  let adjusted = adjust_to_camera cube in
  let (draw_faces, draw_cube) = are_faces_behind adjusted in
  if draw_cube then begin
    let proj = project adjusted screen_wd screen_ht fov in 

    let graphed = to_graphics proj screen_wd screen_ht in 
    set_color (rgb 192 192 192);

    let distances = [|
      max (farthest_pt cube.(0) cube.(1)) (farthest_pt cube.(2) cube.(3));
      max (farthest_pt cube.(4) cube.(5)) (farthest_pt cube.(6) cube.(7));
      max (farthest_pt cube.(0) cube.(1)) (farthest_pt cube.(5) cube.(4));
      max (farthest_pt cube.(1) cube.(2)) (farthest_pt cube.(6) cube.(5));
      max (farthest_pt cube.(2) cube.(3)) (farthest_pt cube.(7) cube.(6));
      max (farthest_pt cube.(3) cube.(0)) (farthest_pt cube.(4) cube.(7));
    |] in

    let order = [|
      [|graphed.(0); graphed.(1); graphed.(2); graphed.(3); graphed.(0)|];
      [|graphed.(4); graphed.(5); graphed.(6); graphed.(7); graphed.(4)|];
      [|graphed.(0); graphed.(1); graphed.(5); graphed.(4); graphed.(0)|];
      [|graphed.(1); graphed.(2); graphed.(6); graphed.(5); graphed.(1)|];
      [|graphed.(2); graphed.(3); graphed.(7); graphed.(6); graphed.(2)|];
      [|graphed.(3); graphed.(0); graphed.(4); graphed.(7); graphed.(3)|];
    |] in 

    (* Note : edge orders must be as following :
      7--------6
      /|       /|
    / |      / |
    4--------5  |
    |  |     |  |
    |  3-----|--2
    | /      | /
    |/       |/
    0--------1
    *)

    for i = 0 to 5 do
      let cur_max = ref distances.(i) in
      let idmax = ref i in
      for j = i to 5 do
        if distances.(j) > !cur_max then begin
          cur_max := distances.(j);
          idmax := j
        end
      done;
      swap distances i !idmax;
      swap order i !idmax;
      swap draw_faces i !idmax;
    done;
    set_line_width 5;
    for i = 0 to 5 do
      if draw_faces.(i) then begin
        let light = max (0.) (1. -. (distances.(i)) /. 7.5) in
        let face_R = int_of_float ((float_of_int r) *. light)
        and face_G = int_of_float ((float_of_int g) *. light)
        and face_B = int_of_float ((float_of_int b) *. light) in
        set_color (rgb face_R face_G face_B);
        fill_poly order.(i);
        set_color black;
        draw_poly_line order.(i);
      end
    done
  end ;;

let draw_cube_p_rev (cube : pt_3d array) screen_wd screen_ht fov r g b =
  let adjusted = adjust_to_camera cube in
  let (draw_faces, draw_cube) = are_faces_behind_rev adjusted in
  if draw_cube then begin
    let proj = project adjusted screen_wd screen_ht fov in 

    let graphed = to_graphics proj screen_wd screen_ht in 
    set_color (rgb 192 192 192);

    let distances = [|
      max (farthest_pt cube.(0) cube.(1)) (farthest_pt cube.(2) cube.(3));
      max (farthest_pt cube.(4) cube.(5)) (farthest_pt cube.(6) cube.(7));
      max (farthest_pt cube.(0) cube.(1)) (farthest_pt cube.(5) cube.(4));
      max (farthest_pt cube.(1) cube.(2)) (farthest_pt cube.(6) cube.(5));
      max (farthest_pt cube.(2) cube.(3)) (farthest_pt cube.(7) cube.(6));
      max (farthest_pt cube.(3) cube.(0)) (farthest_pt cube.(4) cube.(7));
    |] in

    let order = [|
      [|graphed.(0); graphed.(1); graphed.(2); graphed.(3); graphed.(0)|];
      [|graphed.(4); graphed.(5); graphed.(6); graphed.(7); graphed.(4)|];
      [|graphed.(0); graphed.(1); graphed.(5); graphed.(4); graphed.(0)|];
      [|graphed.(1); graphed.(2); graphed.(6); graphed.(5); graphed.(1)|];
      [|graphed.(2); graphed.(3); graphed.(7); graphed.(6); graphed.(2)|];
      [|graphed.(3); graphed.(0); graphed.(4); graphed.(7); graphed.(3)|];
    |] in 

    (* Note : edge orders must be as following :
      7--------6
      /|       /|
     / |      / |
    4--------5  |
    |  |     |  |
    |  3-----|--2
    | /      | /
    |/       |/
    0--------1
    *)

    for i = 0 to 5 do
      let cur_max = ref distances.(i) in
      let idmax = ref i in
      for j = i to 5 do
        if distances.(j) > !cur_max then begin
          cur_max := distances.(j);
          idmax := j
        end
      done;
      swap distances i !idmax;
      swap order i !idmax;
      swap draw_faces i !idmax;
    done;
    set_line_width 5;
    for i = 0 to 5 do
      if draw_faces.(i) then begin
        let light = max (0.) (1. -. (distances.(i)) /. 7.5) in
        let face_R = int_of_float ((float_of_int r) *. light)
        and face_G = int_of_float ((float_of_int g) *. light)
        and face_B = int_of_float ((float_of_int b) *. light) in
        set_color (rgb face_R face_G face_B);
        fill_poly order.(i);
        set_color black;
        draw_poly_line order.(i);
      end
    done
  end ;;

let draw_cube_hollow_p (cube : pt_3d array) screen_wd screen_ht fov r g b=
  let adjusted = adjust_to_camera cube in
  let (draw_faces, draw_cube) = are_faces_behind adjusted in
  let draw_edges = are_edges_behind adjusted in
  if draw_cube then begin
    let proj = project adjusted screen_wd screen_ht fov in 

    let graphed = to_graphics proj screen_wd screen_ht in 
    set_color (rgb 192 192 192);

    let distances = [|
      max (farthest_pt cube.(0) cube.(1)) (farthest_pt cube.(2) cube.(3));
      max (farthest_pt cube.(4) cube.(5)) (farthest_pt cube.(6) cube.(7));
      max (farthest_pt cube.(0) cube.(1)) (farthest_pt cube.(5) cube.(4));
      max (farthest_pt cube.(1) cube.(2)) (farthest_pt cube.(6) cube.(5));
      max (farthest_pt cube.(2) cube.(3)) (farthest_pt cube.(7) cube.(6));
      max (farthest_pt cube.(3) cube.(0)) (farthest_pt cube.(4) cube.(7));
    |] in

    let order = [|
      [|graphed.(0); graphed.(1); graphed.(2); graphed.(3); graphed.(0)|];
      [|graphed.(4); graphed.(5); graphed.(6); graphed.(7); graphed.(4)|];
      [|graphed.(0); graphed.(1); graphed.(5); graphed.(4); graphed.(0)|];
      [|graphed.(1); graphed.(2); graphed.(6); graphed.(5); graphed.(1)|];
      [|graphed.(2); graphed.(3); graphed.(7); graphed.(6); graphed.(2)|];
      [|graphed.(3); graphed.(0); graphed.(4); graphed.(7); graphed.(3)|];
    |] in 

    (* Note : edge orders must be as following :
      7--------6
      /|       /|
    / |      / |
    4--------5  |
    |  |     |  |
    |  3-----|--2
    | /      | /
    |/       |/
    0--------1
    *)

    for i = 0 to 5 do
      let cur_max = ref distances.(i) in
      let idmax = ref i in
      for j = i to 5 do
        if distances.(j) > !cur_max then begin
          cur_max := distances.(j);
          idmax := j
        end
      done;
      swap distances i !idmax;
      swap order i !idmax;
      swap draw_faces i !idmax;
      swap draw_edges i !idmax
    done;
    set_line_width 5;
    set_color (rgb r g b);
    for i = 0 to 5 do
      for j = 0 to 3 do
        if draw_edges.(i).(j) then begin
          draw_poly_line [|order.(i).(j) ; order.(i).((j+1) mod 4)|];
        end
      done
    done
  end ;;

let sum_x (poly : pt_3d array) =
  let res = ref 0. in
  for i = 0 to (Array.length poly -1) do
    res := !res +. poly.(i).x
  done;
  !res /. (float_of_int (Array.length poly));;

let sum_y (poly : pt_3d array) =
  let res = ref 0. in
  for i = 0 to (Array.length poly -1) do
    res := !res +. poly.(i).y
  done;
  !res /. (float_of_int (Array.length poly));;

let sum_z (poly : pt_3d array) =
  let res = ref 0. in
  for i = 0 to (Array.length poly -1) do
    res := !res +. poly.(i).z
  done;
  !res /. (float_of_int (Array.length poly)) ;;

let cube_dist (c : pt_3d array) =
  let mid_pt = {
    x = sum_x c;
    y = sum_y c;
    z = sum_z c
  }
  in dist_from_camera mid_pt ;;

let draw_multiples_cubes_colored (cubes : pt_3d array dynamic) rs gs bs screen_wd screen_ht fov render_dist = 
  let n = cubes.len in

  let distances = Array.make n 0. in

  for i = 0 to n-1 do
    distances.(i) <- cube_dist cubes.tab.(i);
  done ;

  for i = 0 to n-1 do
    let cur_max = ref distances.(i) in
    let idmax = ref i in
    for j = i to n-1 do
      if distances.(j) > !cur_max then begin
        cur_max := distances.(j);
        idmax := j;
      end
    done;
    swap distances i !idmax;
    swap cubes.tab i !idmax;
  done;
  for i = 0 to n-1 do
    if distances.(i) <= (float_of_int render_dist) then begin
      draw_cube_p cubes.tab.(i) screen_wd screen_ht fov rs.tab.(i) gs.tab.(i) bs.tab.(i)
    end
  done ;;

let create_cube x0' y0' z0' sz' =
  let x0 = float_of_int x0'
  and y0 = float_of_int y0'
  and z0 = float_of_int z0'
  and s = float_of_int sz' in
  let res = [|
    {x = x0 ; y = y0 ; z = z0};
    {x = x0 +. s ; y = y0 ; z = z0};
    {x = x0 +. s ; y = y0 +. s ; z = z0};
    {x = x0 ; y = y0 +. s ; z = z0};
    {x = x0 ; y = y0 ; z = z0 +. s};
    {x = x0 +. s ; y = y0 ; z = z0 +. s};
    {x = x0 +. s ; y = y0 +. s ; z = z0 +. s};
    {x = x0 ; y = y0 +. s ; z = z0 +. s}
  |]
  in res ;;

let fov = 90 ;;

(*
      7--------6
      /|       /|
     / |      / |
    4--------5  |
    |  |     |  |
    |  3-----|--2
    | /      | /
    |/       |/
    0--------1

[|graphed.(0); graphed.(1); graphed.(2); graphed.(3); graphed.(0)|];
[|graphed.(4); graphed.(5); graphed.(6); graphed.(7); graphed.(4)|];
[|graphed.(0); graphed.(1); graphed.(5); graphed.(4); graphed.(0)|];
[|graphed.(1); graphed.(2); graphed.(6); graphed.(5); graphed.(1)|];
[|graphed.(2); graphed.(3); graphed.(7); graphed.(6); graphed.(2)|];
[|graphed.(3); graphed.(0); graphed.(4); graphed.(7); graphed.(3)|];

*)

let print_cube (cube : pt_3d array) =
  for j = 0 to 7 do
    Printf.printf "    {%f, %f, %f}\n" cube.(j).x cube.(j).y cube.(j).z
  done;
  Stdlib.print_endline " " ;;

let get1char () =
    let termio = Unix.tcgetattr Unix.stdin in
    let () =
        Unix.tcsetattr Unix.stdin Unix.TCSADRAIN
            { termio with Unix.c_icanon = false } in
    let res = input_char stdin in
    Unix.tcsetattr Unix.stdin Unix.TCSADRAIN termio;
    res ;;

(* ---------------------------------------------------- *)
(* ---------------------------------------------------- *)
(* ---------------------------------------------------- *)
(* ---------------------------------------------------- *)

type tile = Free | Wall | Crate | Exit | Craxit | Camera ;;

let width = 15
and height = 15
and depth = 15 ;;
(* dimensions *)

let render_distance = 7 ;;

let laby = Array.make width [|[||]|] ;;
for i = 0 to width -1 do
  laby.(i) <- Array.make_matrix height depth Wall
done ;;

let n_walls = ref (width*height*depth) ;;

let is_collision_cube (cam_coords : pt_3d) (cube : pt_3d array) = 
  (*Printf.printf "testing cube [%b, %b, %b, %b, %b, %b] :" 
    (cube.(0).x >= (-. cam_coords.x)) 
    (cube.(0).y >= (-. cam_coords.x)) 
    (cube.(0).z >= cam_coords.z)
    (cube.(6).x <= (-. cam_coords.x))
    (cube.(6).y <= (-. cam_coords.y))
    (cube.(6).z <= cam_coords.z) ;
  Printf.printf "\n||{%f, %f, %f}, {%f, %f, %f}||\n" cube.(0).x cube.(0).y cube.(0).z cube.(6).x cube.(6).y cube.(6).z;
  Printf.printf "with coords (%f, %f, %f)\n" (-. cam_coords.x) (-. cam_coords.y) cam_coords.z;
  Stdlib.print_endline " ";*)
  cube.(0).x <= (-. cam_coords.x) &&
  cube.(0).y <= (-. cam_coords.y) &&
  cube.(0).z <= cam_coords.z &&
  cube.(6).x >= (-. cam_coords.x) &&
  cube.(6).y >= (-. cam_coords.y) &&
  cube.(6).z >= cam_coords.z ;;

let is_collision (cam_coords : pt_3d) (cubes : pt_3d array array) = 
  let res = ref false in
  let n = !n_walls in
  
  let distances = Array.make n 0. in
  
  for i = 0 to n-1 do
    distances.(i) <- cube_dist cubes.(i);
  done ;

  for i = 0 to n-1 do
    if not !res && distances.(i) < 2. then
      res := is_collision_cube cam_coords cubes.(i)
  done;
  !res ;;

let convert_laby laby =
  let width = Array.length laby
  and height = Array.length laby.(0)
  and depth = Array.length laby.(0).(0) in

  let cubes = dyn_create (create_cube 0 0 0 0)
  and reds = dyn_create 0
  and greens = dyn_create 0
  and blues = dyn_create 0 in

  for w = 0 to width-1 do
    for h = 0 to height-1 do
      for d = 0 to depth-1 do
        if laby.(w).(h).(d) <> Free then begin
          (*Printf.printf "added (%d, %d, %d)" w h d;
          Stdlib.print_endline " ";*)
          dyn_append cubes (create_cube w h d 1);
          dyn_append reds 212;
          dyn_append greens 212;
          dyn_append blues 212;
        end
      done
    done
  done;
  (cubes, reds, greens, blues) ;;

let cheesify (laby : tile array array array) =
  let width = Array.length laby
  and height = Array.length laby.(0)
  and depth = Array.length laby.(0).(0) in

  for w = 0 to width-1 do
    for h = 0 to height-1 do
      for d = 0 to depth-1 do
        let rand_w = Random.int width
        and rand_h = Random.int height
        and rand_d = Random.int depth in
        (*Printf.printf "chose (%d, %d, %d)" rand_w rand_h rand_d;
        Stdlib.print_endline " ";*)
        if laby.(rand_w).(rand_h).(rand_d) <> Free then begin
          laby.(rand_w).(rand_h).(rand_d) <- Free;
          decr n_walls
        end
      done
    done
  done;;

let print_cubes (cubes : pt_3d array array) =
  for i = 0 to !n_walls -1 do
    for j = 0 to 7 do
      Printf.printf "    {%f, %f, %f}\n" cubes.(i).(j).x cubes.(i).(j).y cubes.(i).(j).z
    done;
    Stdlib.print_endline " "
  done ;;

(* z q s d for movement, p to go up, m to go down, a to rotate left, e to rotate right *)
let rec move_cam (cubes : pt_3d array array) b c =(* Printf.printf "[%b]" b; Stdlib.print_endline " " ; *)match c with
  | 'z' -> 
    camera_xyz.z <- camera_xyz.z +. Float.cos ((float_of_int !camera_angle_y) *. 3.1415926535 /. 180.);
    camera_xyz.x <- camera_xyz.x +. Float.sin ((float_of_int !camera_angle_y) *. 3.1415926535 /. 180.);
    if b && (is_collision camera_xyz cubes) then move_cam cubes false 's'
  | 'q' -> 
    camera_xyz.z <- camera_xyz.z +. Float.cos (((float_of_int !camera_angle_y) +. 90.) *. 3.1415926535 /. 180.);
    camera_xyz.x <- camera_xyz.x +. Float.sin (((float_of_int !camera_angle_y) +. 90.) *. 3.1415926535 /. 180.);
    if b && (is_collision camera_xyz cubes) then move_cam cubes false 'q'
  | 's' -> 
    camera_xyz.z <- camera_xyz.z -. Float.cos ((float_of_int !camera_angle_y) *. 3.1415926535 /. 180.);
    camera_xyz.x <- camera_xyz.x -. Float.sin ((float_of_int !camera_angle_y) *. 3.1415926535 /. 180.);
    if b && (is_collision camera_xyz cubes) then move_cam cubes false 'z'
  | 'd' -> 
    camera_xyz.z <- camera_xyz.z +. Float.cos (((float_of_int !camera_angle_y) -. 90.) *. 3.1415926535 /. 180.);
    camera_xyz.x <- camera_xyz.x +. Float.sin (((float_of_int !camera_angle_y) -. 90.) *. 3.1415926535 /. 180.);
    if b && (is_collision camera_xyz cubes) then move_cam cubes false 'q'
  | 'p' -> 
    camera_xyz.y <- camera_xyz.y -. 1.;
    if b && (is_collision camera_xyz cubes) then move_cam cubes false 'm'
  | 'm' -> 
    camera_xyz.y <- camera_xyz.y +. 1.;
    if b && (is_collision camera_xyz cubes) then move_cam cubes false 'p'
  | 'a' -> camera_angle_y := !camera_angle_y + 15
  | 'e' -> camera_angle_y := !camera_angle_y - 15
  | _ -> () ;;

let play laby =
  cheesify laby;
  let (cs, rs, gs, bs) = convert_laby laby in

  camera_xyz.z <- -. (1.5) ;
  camera_xyz.x <- -. (float_of_int width) /. 2. ;
  camera_xyz.y <- -. (float_of_int height) /. 2. ;

  (*print_cubes cs ;*)

  while true do
    auto_synchronize false;
    open_graph " 1500x1000";
    set_color black;
    fill_poly [|(0, 0); (1500, 0); (1500, 1000); (0, 1000); (0, 0)|];
    set_color white;

    draw_multiples_cubes_colored cs rs gs bs __width__ __height__ fov render_distance ;

    auto_synchronize true;

    Printf.printf "current pos : (%f, %f, %f)" (-. camera_xyz.x) (-. camera_xyz.y) camera_xyz.z;
    Stdlib.print_endline " ";

    let usr_input = get1char () in 
    move_cam cs.tab true usr_input
  done ;;

play laby ;;