open Graphics ;;

Random.self_init () ;;

let __width__ = 1500
and __height__ = 1000 ;;

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

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

let render_distance = 7 ;;
let chunk_dist = 2 ;;

let chunk_size = 4 ;;
let chunk_size_f = float_of_int chunk_size ;;

let density = 50 ;;

let speed_multiplier = 1.5 ;;

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

(*

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

*)

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

type texture = {mutable width : int ; mutable height : int ; mutable arr_red : int array array ; mutable arr_green : int array array ; mutable arr_blue : int array array} ;;

let parse_texture filename =
  let ptr = open_in filename in
  let tex = {width = 0 ; height = 0; arr_red = Array.make_matrix 1 1 0; arr_green = Array.make_matrix 1 1 0; arr_blue = Array.make_matrix 1 1 0} in
  try
    let buffer = ref 0 in

    let side = ref 0 in

    (* read dimensions *)
    while !side <> 2 do
      let c = input_char ptr in
      let code = Char.code c in
      if code >= 48 && code <= 57 then begin
        buffer := !buffer * 10;
        buffer := !buffer + code - 48
      end
      else begin
        if !side = 0 then
          tex.width <- !buffer
        else
          tex.height <- !buffer;

        incr side;
        buffer := 0
      end
    done;

    Printf.printf "size is (%d, %d)" tex.width tex.height;
    Stdlib.print_endline " ";

    tex.arr_red <- Array.make_matrix (tex.width) (tex.height) 0;
    tex.arr_green <- Array.make_matrix (tex.width) (tex.height) 0;
    tex.arr_blue <- Array.make_matrix (tex.width) (tex.height) 0;

    (* read data*)
    let cred = ref 0
    and cgreen = ref 0
    and cblue = ref 0 in

    let which_color = ref 0 in

    let cur_w = ref 0
    and cur_h = ref 0 in

    while true do
      let c = input_char ptr in
      let code = Char.code c in
      if code >= 48 && code <= 57 then begin (* integer *)
        buffer := !buffer * 10;
        buffer := !buffer + code - 48
      end
      else if c = ',' then begin
        if !which_color = 0 then
          cred := !buffer
        else 
          cgreen := !buffer;
        (* blue is not seen here *)

        incr which_color;
        buffer := 0
      end
      else if c = ' ' then begin
        cblue := !buffer;

        tex.arr_red.(!cur_w).(!cur_h) <- !cred;
        tex.arr_green.(!cur_w).(!cur_h) <- !cgreen;
        tex.arr_blue.(!cur_w).(!cur_h) <- !cblue;

        incr cur_w;
        buffer := 0;
        which_color := 0
      end
      else if c = '\n' then begin
        cblue := !buffer;
        
        tex.arr_red.(!cur_w).(!cur_h) <- !cred;
        tex.arr_green.(!cur_w).(!cur_h) <- !cgreen;
        tex.arr_blue.(!cur_w).(!cur_h) <- !cblue;

        incr cur_h;
        cur_w := 0;
        buffer := 0;
        which_color := 0
      end
    done;
    failwith "Oh so while true can exit on its own..."
  with
    | End_of_file -> 
      close_in ptr ;
      Printf.printf "Successfully parsed texture ";
      Printf.printf "'%s'" filename;
      Stdlib.print_endline " ";
      tex
    | exn -> close_in ptr ; raise exn ;;

let stone = parse_texture "output.txt" ;;

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

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

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

type coloredCube = {cube : pt_3d array ; red : int ; green : int ; blue : int} ;;

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

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 abs x = if x >= 0 then x else -x ;;
let absf x = if x >= 0. then x else -.(x) ;;

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

(* ------------------------------------------------------------- *)
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 rec ln10 n = match n with
    | k when k < 0 -> failwith "Are you sure about that ?"
    | k when k < 10 -> 0
    | k -> 1 + ln10 (k/10) ;;

let draw_integer_alignedleft x0 y n0 len =
  (* 7-seg display 2 *)
  set_line_width (max 1 (len/4));
  let n = ref n0 in
  let size = ln10 (abs n0) in

  let cur_x = ref (x0 + size*(len*11/7)) in

  if !n < 0 then begin
      n := !n * (-1);
      draw_poly_line [|(x0, y); (x0+len, y)|];
      cur_x := !cur_x + (len*11/7)
  end;

  for i = 0 to size do
      let x = !cur_x in
      if Array.mem (!n mod 10) [|0; 4; 5; 6; 7; 8; 9|] then
          draw_poly_line [|(x, y+len); (x, y)|];

      if Array.mem (!n mod 10) [|0; 2; 3; 5; 6; 7; 8; 9|] then
          draw_poly_line [|(x, y+len); (x+len, y+len)|];

      if Array.mem (!n mod 10) [|0; 1; 2; 3; 4; 7; 8; 9|] then
          draw_poly_line [|(x+len, y+len); (x+len, y)|];

      if Array.mem (!n mod 10) [|2; 3; 4; 5; 6; 8; 9|] then
          draw_poly_line [|(x, y); (x+len, y)|];

      if Array.mem (!n mod 10) [|0; 1; 3; 4; 5; 6; 7; 8; 9|] then
          draw_poly_line [|(x+len, y-len); (x+len, y)|];

      if Array.mem (!n mod 10) [|0; 2; 3; 5; 6; 8; 9|] then
          draw_poly_line [|(x, y-len); (x+len, y-len)|];

      if Array.mem (!n mod 10) [|0; 2; 6; 8|] then
          draw_poly_line [|(x, y-len); (x, y)|];

      n := !n/10;
      cur_x := !cur_x - (len*11/7);
  done ;;

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

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 adapt_to_dims x y =
 (max 0 (min __width__ x), max 0 (min __height__ y)) ;;

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) <- adapt_to_dims proj_x proj_y;*)
    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.4 *. (sign shape.(k).x)) *. Float.tan (((float_of_int fov) *. 3.14159265358 /. 180.) /. 2.));
      res.(k).y <- (absf shape.(k).y) /. ((0.4 *. (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 adjust_pt (pt : pt_3d) =
  let tpt = {x = pt.x +. camera_xyz.x ; y = pt.y +. camera_xyz.y ; z = pt.z -. camera_xyz.z} in
  {
    x = tpt.x *. Float.cos ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.) +. tpt.z *. Float.sin ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.);
    y = tpt.y;
    z = tpt.z *. Float.cos ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.) -. tpt.x *. Float.sin ((float_of_int !camera_angle_y) *. 3.14159255358 /. 180.)
  } ;;

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 convex_seg x1 x2 theta maxtheta =
  let ratio = (float_of_int theta) /. (float_of_int maxtheta) in
  int_of_float ((1. -. ratio) *. (float_of_int x1) +. ratio *. (float_of_int x2)) ;;

let convex_pt (p1 : int * int) (p2 : int * int) theta maxtheta =
  let ratio = (float_of_int theta) /. (float_of_int maxtheta) in
  let mid_x = int_of_float ((1. -. ratio) *. (float_of_int (fst p1)) +. ratio *. (float_of_int (fst p2)))
  and mid_y = int_of_float ((1. -. ratio) *. (float_of_int (snd p1)) +. ratio *. (float_of_int (snd p2))) in
  (mid_x, mid_y) ;;

let convex_3d (p1 : pt_3d) (p2 : pt_3d) theta maxtheta =
  let ratio = (float_of_int theta) /. (float_of_int maxtheta) in
  {
    x = (1. -. ratio) *. p1.x +. ratio *. p2.x;
    y = (1. -. ratio) *. p1.y +. ratio *. p2.y;
    z = (1. -. ratio) *. p1.z +. ratio *. p2.z
  } ;;

let ctc_one x =
  if x >= 0 then
    x / chunk_size
  else
    x / chunk_size -1 ;;
  
let ctcf_one x =
  if x >= 0. then
    int_of_float (x /. chunk_size_f)
  else
    int_of_float (x /. chunk_size_f) -1 ;;

let coords_to_chunk x y z =
  (ctc_one x, ctc_one y, ctc_one z) ;;

let coords_to_chunk_f x y z =
  (ctcf_one x, ctcf_one y, ctcf_one z) ;;

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

let is_in_cube (pt : pt_3d) (cube : pt_3d array) =
  (* cube and pt are relative to the camera *)
  (*Printf.printf "            comparing with cube    : (%f, %f, %f)" cube.(0).x cube.(0).y cube.(0).z;
  Stdlib.print_endline " ";*)
  (cube.(0).x <= pt.x) &&
  (cube.(0).y <= pt.y) &&
  (cube.(0).z <= pt.z) &&
  (cube.(6).x >= pt.x) &&
  (cube.(6).y >= pt.y) &&
  (cube.(6).z >= pt.z) ;;

let seg_len (p1 : pt_3d) (p2 : pt_3d) =
  Float.sqrt ((sq (p1.x -. p2.x)) +. (sq (p1.y -. p2.y)) +. (sq (p1.z -. p2.z))) ;;

exception ReturnBool of bool ;;
let is_visible (pt0 : pt_3d) (hash : (int * int * int, coloredCube dynamic) Hashtbl.t) =
  try
    let pt = pt0 in
    (*Printf.printf "    current pt : (%f, %f, %f)\n" pt.x pt.y pt.z;
    Printf.printf "    camera  pt : (%f, %f, %f)\n" (-. camera_xyz.x) (-. camera_xyz.y) camera_xyz.z;*)

    let segment_length = dist_from_camera pt in
    let n_iter = int_of_float (segment_length /. 1.7) in

    (*Printf.printf "    distance : %f\n" segment_length;
    Printf.printf "    n_iter : %d" n_iter;
    Stdlib.print_endline " ";*)

    for i = 1 to n_iter -1 do
      let cur_pt = convex_3d pt {x = -. camera_xyz.x ; y = -. camera_xyz.y ; z = camera_xyz.z} i n_iter in
      let (ch_x, ch_y, ch_z) = coords_to_chunk_f cur_pt.x cur_pt.y cur_pt.z in

      (*Printf.printf "        regarding pt : (%f, %f, %f) in chunk (%d, %d, %d)" cur_pt.x cur_pt.y cur_pt.z ch_x ch_y ch_z;
      Stdlib.print_endline " ";*)

      let cubes = Hashtbl.find hash (ch_x, ch_y, ch_z) in
      for c = 0 to cubes.len -1 do
        if is_in_cube cur_pt cubes.tab.(c).cube then
          raise (ReturnBool false)
      done
    done;
    (*Stdlib.print_endline "    No collision detected";*)
    true
  with
    | Not_found ->  (*Stdlib.print_endline "        EMPTY"; *)true 
    | ReturnBool b -> (*Printf.printf "    Aborted to %b" b ; Stdlib.print_endline " "; *)b ;;

let is_visible_poly (cube : pt_3d array) (hash : (int * int * int, coloredCube dynamic) Hashtbl.t) =
  (*Printf.printf "cube (%f, %f, %f)" cube.(0).x cube.(0).y cube.(0).z;
  Printf.printf "[of length %d]" (Array.length cube);
  Stdlib.print_endline " ";*)
  let res = ref false in
  for i = 0 to Array.length cube -1 do
    res := !res || (is_visible cube.(i) hash)
  done;
  (*Printf.printf "yielding %b" !res;
  Stdlib.print_endline " "; *)
  !res ;;

let is_visible_cube (cube : pt_3d array) (hash : (int * int * int, coloredCube dynamic) Hashtbl.t) =
  let r0 = is_visible cube.(0) hash
  and r1 = is_visible cube.(1) hash
  and r2 = is_visible cube.(2) hash
  and r3 = is_visible cube.(3) hash
  and r4 = is_visible cube.(4) hash
  and r5 = is_visible cube.(5) hash
  and r6 = is_visible cube.(6) hash
  and r7 = is_visible cube.(7) hash in
  (r1 || r2 || r3 || r4 || r5 || r6 || r7 || r0, [|
    r0 || r1 || r2 || r3 ;
    r4 || r5 || r6 || r7 ;
    r0 || r1 || r5 || r4 ;
    r1 || r2 || r6 || r5 ;
    r2 || r3 || r7 || r6 ;
    r3 || r0 || r4 || r7
  |]) ;;

let draw_texture (rect : (int * int) array) (text : texture) light = 
  (*set_color white;
  fill_poly rect ;;*)
  Stdlib.print_endline "{";
  for i = 0 to text.width -1 do
    for j = 0 to text.height -1 do
      if true then begin
        Printf.printf "    (((%f)))" light;
        Stdlib.print_endline " ";
        let face_R = int_of_float ((float_of_int text.arr_red.(i).(j)) *. light)
        and face_G = int_of_float ((float_of_int text.arr_green.(i).(j)) *. light)
        and face_B = int_of_float ((float_of_int text.arr_blue.(i).(j)) *. light) in
        set_color (rgb face_R face_G face_B);

        let pt_a = convex_pt rect.(0) rect.(1) i text.width
        and pt_b = convex_pt rect.(0) rect.(1) (i+1) text.width

        and pt_e = convex_pt rect.(3) rect.(2) (i+1) text.width
        and pt_f = convex_pt rect.(3) rect.(2) i text.width in

        let bot_left = convex_pt pt_a pt_f j text.height
        and bot_right = convex_pt pt_b pt_e j text.height
        and top_left = convex_pt pt_a pt_f (j+1) text.height
        and top_right = convex_pt pt_b pt_e (j+1) text.height in
        fill_poly [|bot_left; bot_right; top_right; top_left|]
      end
    done
  done;
  Stdlib.print_endline "}" ;;

let draw_cube_p (cube : pt_3d array) (hash : (int * int * int, coloredCube dynamic) Hashtbl.t) 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_cube_2, draw_faces_2) = is_visible_cube cube hash in
  if draw_cube && draw_cube_2 then begin
    (*Printf.printf "drawing cube (%f, %f, %f)" cube.(0).x cube.(0).y cube.(0).z ;
    Stdlib.print_endline " ";*)
    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.(4); graphed.(5); graphed.(6); graphed.(7)|];
      [|graphed.(0); graphed.(1); graphed.(5); graphed.(4)|];
      [|graphed.(1); graphed.(2); graphed.(6); graphed.(5)|];
      [|graphed.(2); graphed.(3); graphed.(7); graphed.(6)|];
      [|graphed.(3); graphed.(0); graphed.(4); graphed.(7)|];
    |] 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_faces_2 i !idmax;
    done;
    set_line_width 2;
    for i = 0 to 5 do
      if draw_faces.(i) && draw_faces_2.(i) then begin
        let light = max (0.) (1. -. (distances.(i)) /. 12.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);
        (*draw_texture order.(i) stone light ;*)
        set_color black;
        draw_poly_line order.(i);
      end
    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 hash 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) hash screen_wd screen_ht fov rs.tab.(i) gs.tab.(i) bs.tab.(i)
    end
  done ;;

let draw_multiples_cubes_colored_hash (dyna : coloredCube dynamic) (hash : (int * int * int, coloredCube dynamic) Hashtbl.t) screen_wd screen_ht fov = 
  let n = dyna.len in

  (*Printf.printf ">> %d <<" n;
  Stdlib.print_endline " ";*)

  let distances = Array.make n 0. in

  for i = 0 to n-1 do
    distances.(i) <- cube_dist dyna.tab.(i).cube;
  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 dyna.tab i !idmax;
  done;
  for i = 0 to n-1 do
    (*Printf.printf "drawing (%f, %f, %f)" (dyna.tab.(i).cube.(0).x) (dyna.tab.(i).cube.(0).y) (dyna.tab.(i).cube.(0).z);
    Stdlib.print_endline " ";*)
    draw_cube_p (dyna.tab.(i).cube) hash screen_wd screen_ht fov (dyna.tab.(i).red) (dyna.tab.(i).green) (dyna.tab.(i).blue)
  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 ;;

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

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) = 
  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 is_collision_hash (cam_coords : pt_3d) (cubes : coloredCube dynamic) = 
  let res = ref false in
  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).cube;
  done ;

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

let is_collision_hash_2 (cam_coords : pt_3d) (rcubes : (coloredCube dynamic) option) = match rcubes with
  | None -> false
  | Some cubes -> begin
    let res = ref false in
    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).cube;
    done ;

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

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 chunkify_2 laby sz = 
  let width = Array.length laby
  and height = Array.length laby.(0)
  and depth = Array.length laby.(0).(0) in

  let cubes = Hashtbl.create 300 in

  let add_to_table w h d r g b = 
    (*Printf.printf "(%d, %d, %d) (%d, %d, %d)\n" w h d cw ch cd;*)
    let (cw, ch, cd) = coords_to_chunk (w*sz) (h*sz) (d*sz) in
    match Hashtbl.find_opt cubes (cw, ch, cd) with
    | None -> begin
      (*Printf.printf "created cube (%d, %d, %d) (%d)\n" (w*sz) (h*sz) (d*sz) sz;
      Printf.printf "in chunk (%d, %d, %d)\n" cw ch cd;
      Stdlib.print_endline " ";*)
      let dyna = dyn_create {cube = create_cube (w*sz) (h*sz) (d*sz) sz; red = r; green = g; blue = b} in
      dyn_append dyna {cube = create_cube (w*sz) (h*sz) (d*sz) sz; red = r; green = g; blue = b};
      Hashtbl.add cubes (cw, ch, cd) dyna
    end
    | Some dyna -> begin
      (*Printf.printf "created cube (%d, %d, %d) (%d)\n" (w*sz) (h*sz) (d*sz) sz;
      Printf.printf "in chunk (%d, %d, %d)\n" cw ch cd;
      Stdlib.print_endline " ";*)
      Hashtbl.remove cubes (cw, ch, cd);
      dyn_append dyna {cube = create_cube (w*sz) (h*sz) (d*sz) sz; red = r; green = g; blue = b};
      Hashtbl.add cubes (cw, ch, cd) dyna
    end
  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
          add_to_table w h d 220 220 220
        end
      done
    done
  done;

  cubes ;;

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 = 1 + Random.int (height-1)
        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 rec move_cam_hash_2 hash cx cy cz b c =(* Printf.printf "[%b]" b; Stdlib.print_endline " " ; *)match c with
  | 'z' -> 
    camera_xyz.z <- camera_xyz.z +. speed_multiplier *. Float.cos ((float_of_int !camera_angle_y) *. 3.1415926535 /. 180.);
    camera_xyz.x <- camera_xyz.x +. speed_multiplier *. Float.sin ((float_of_int !camera_angle_y) *. 3.1415926535 /. 180.);
    if b && (
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx+1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx-1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy+1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy-1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz+1))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz-1)))
    ) then move_cam_hash_2 hash cx cy cz false 's'
  | 'q' -> 
    camera_xyz.z <- camera_xyz.z +. speed_multiplier *. Float.cos (((float_of_int !camera_angle_y) +. 90.) *. 3.1415926535 /. 180.);
    camera_xyz.x <- camera_xyz.x +. speed_multiplier *. Float.sin (((float_of_int !camera_angle_y) +. 90.) *. 3.1415926535 /. 180.);
    if b && (
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx+1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx-1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy+1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy-1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz+1))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz-1)))
    ) then move_cam_hash_2 hash cx cy cz false 'q'
  | 's' -> 
    camera_xyz.z <- camera_xyz.z -. speed_multiplier *. Float.cos ((float_of_int !camera_angle_y) *. 3.1415926535 /. 180.);
    camera_xyz.x <- camera_xyz.x -. speed_multiplier *. Float.sin ((float_of_int !camera_angle_y) *. 3.1415926535 /. 180.);
    if b && (
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx+1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx-1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy+1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy-1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz+1))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz-1)))
    ) then move_cam_hash_2 hash cx cy cz false 'z'
  | 'd' -> 
    camera_xyz.z <- camera_xyz.z +. speed_multiplier *. Float.cos (((float_of_int !camera_angle_y) -. 90.) *. 3.1415926535 /. 180.);
    camera_xyz.x <- camera_xyz.x +. speed_multiplier *. Float.sin (((float_of_int !camera_angle_y) -. 90.) *. 3.1415926535 /. 180.);
    if b && (
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx+1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx-1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy+1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy-1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz+1))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz-1)))
    ) then move_cam_hash_2 hash cx cy cz false 'q'
  | 'p' -> 
    camera_xyz.y <- camera_xyz.y -. speed_multiplier ;
    if b && (
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx+1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx-1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy+1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy-1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz+1))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz-1)))
    ) then move_cam_hash_2 hash cx cy cz false 'm'
  | 'm' -> 
    camera_xyz.y <- camera_xyz.y +. speed_multiplier ;
    if b && (
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx+1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx-1,cy,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy+1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy-1,cz))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz+1))) ||
      (is_collision_hash_2 camera_xyz (Hashtbl.find_opt hash (cx,cy,cz-1)))
    ) then move_cam_hash_2 hash cx cy cz false 'p'
  | 'a' -> camera_angle_y := !camera_angle_y + 30
  | 'e' -> camera_angle_y := !camera_angle_y - 30
  | _ -> () ;;

let initialize_chunk hash ch_x ch_y ch_z =
  let dyna = dyn_create {cube = create_cube 0 0 0 1; red = 33; green = 33; blue = 22} in
  for i = 0 to 3 do
    for j = 0 to 3 do
      dyn_append dyna {cube = create_cube (4*ch_x +i) (4*ch_y + (i + j) mod 4) (4*ch_z+j) 1; red = 32; green = 32; blue = 200};
    done
  done;
  Hashtbl.add hash (ch_x, ch_y, ch_z) dyna ;;

let initialize_flat hash ch_x ch_y ch_z =
  let dyna = dyn_create {cube = create_cube 0 0 0 1; red = 33; green = 33; blue = 22} in
  for i = 0 to 3 do
    for j = 0 to 3 do
      for k = 0 to 3 do
        dyn_append dyna {cube = create_cube (4*ch_x +i) (4*ch_y +k) (4*ch_z +j) 1; red = 32; green = 222; blue = 32};
      done
    done
  done;
  Hashtbl.add hash (ch_x, ch_y, ch_z) dyna ;;

let init_random hash ch_x ch_y ch_z =
  let dyna = dyn_create {cube = create_cube 0 0 0 1; red = 33; green = 33; blue = 22} in
  if (Random.int 101) < density then
    dyn_append dyna {cube = create_cube (4*ch_x) (4*ch_y) (4*ch_z) 4; red = 250 ; green = 250 ; blue = 250};
  Hashtbl.add hash (ch_x, ch_y, ch_z) dyna ;;

let rec manage_unexisting_chunk hash ch_x ch_y ch_z screen_wd screen_ht fov =
  try
    draw_multiples_cubes_colored_hash (Hashtbl.find hash (ch_x, ch_y, ch_z)) hash screen_wd screen_ht fov
  with
    | Not_found when ch_y = (-64)-> initialize_flat hash ch_x ch_y ch_z ; manage_unexisting_chunk hash ch_x ch_y ch_z screen_wd screen_ht fov
    | Not_found when ch_y = 64-> initialize_flat hash ch_x ch_y ch_z ; manage_unexisting_chunk hash ch_x ch_y ch_z screen_wd screen_ht fov
    | Not_found -> init_random hash ch_x ch_y ch_z ; manage_unexisting_chunk hash ch_x ch_y ch_z screen_wd screen_ht fov ;;

let render_chunks hash camx camy camz ch_distance screen_wd screen_ht fov =
  let arr = Array.make ((2*ch_distance + 1)*(2*ch_distance + 1)*(2*ch_distance + 1)) ((0, 0), (0, 99)) in
  let id = ref 0 in
  for i = -ch_distance to ch_distance do
    for j = -ch_distance to ch_distance do
      for k = -ch_distance to ch_distance do
        arr.(!id) <- ((camx+i, camy+j), (camz+k, (abs i) + (abs j) + (abs k)));
        incr id
      done
    done
  done;
  let sort_fct elt1 elt2 =
    (*- (snd (snd elt2)) + (snd (snd elt1))*)
    (snd (snd elt2)) - (snd (snd elt1))
  in

  Array.sort sort_fct arr ;
  for i = 0 to (Array.length arr -1) do
    (*Printf.printf "[%d, %d, %d] (%d)" (fst (fst arr.(i))) (snd (fst arr.(i))) (fst (snd arr.(i))) (snd (snd arr.(i)));
    Stdlib.print_endline " ";*)
    manage_unexisting_chunk hash (fst (fst arr.(i))) (snd (fst arr.(i))) (fst (snd arr.(i))) screen_wd screen_ht fov ;
  done ;;

let get1char_plus () =
  if key_pressed () then
    read_key ()
  else
    '@' ;;

let play_dos laby =
  try
    Stdlib.print_endline "Building terrain...";
    (*cheesify laby;*)

    Stdlib.print_endline "Converting terrain...";
    (*let hash = chunkify_2 laby 2 in*)
    let hash = Hashtbl.create 100 in

    let dyna = dyn_create {cube = create_cube 0 0 0 1 ; red = 30 ; green = 30 ; blue = 30} in
    Hashtbl.add hash (0, 0, 0) dyna ;

    (*print_cubes cs ;*)

    let redraw = ref true in

    let ch_x = ref 0
    and ch_y = ref 0
    and ch_z = ref 0 in

    camera_xyz.x <- (-. 2.) ;
    camera_xyz.y <- (-. 2.) ;
    camera_xyz.z <- 2. ;

    while true do
      if !redraw then begin
        auto_synchronize false;
        open_graph " 1500x1000";
        set_color black;
        fill_poly [|(0, 0); (__width__, 0); (__width__, __height__); (0, __height__); (0, 0)|];

        let (cx, cy, cz) = coords_to_chunk_f (-. camera_xyz.x) (-. camera_xyz.y) camera_xyz.z in
        ch_x := cx ; ch_y := cy ; ch_z := cz;

        render_chunks hash !ch_x !ch_y !ch_z chunk_dist __width__ __height__ fov ;

        set_color white;
        draw_integer_alignedleft 10 (__height__ - 30) (-int_of_float camera_xyz.x) 25;
        draw_integer_alignedleft 10 (__height__ - 90) (-int_of_float camera_xyz.y) 25;
        draw_integer_alignedleft 10 (__height__ - 150) (int_of_float camera_xyz.z) 25;

        auto_synchronize true;
      end;
      redraw := false;

      let usr_input = get1char_plus () in 
      if usr_input <> '@' then begin
        move_cam_hash_2 hash !ch_x !ch_y !ch_z true usr_input;
        redraw := true
      end
    done ;
    ()
  with
    | Not_found -> Stdlib.print_endline "Wait that's illegal\n" ;;

play_dos laby ;;