structure BinTree =
struct
  datatype 'a bintree =
    NODE of
      { x: int
      , y: int
      , ex: int
      , ey: int
      , data: 'a
      , left: 'a bintree
      , right: 'a bintree
      }
  | LEAF

  val empty = LEAF

  fun insert (newItem as {x, y, ex, ey, data}, tree) =
    case tree of
      LEAF =>
        NODE
          { x = x
          , y = y
          , ex = ex
          , ey = ey
          , data = data
          , left = LEAF
          , right = LEAF
          }
    | NODE {x = ox, y = oy, ex = oex, ey = oey, data = oldData, left, right} =>
        let
          val dir =
            if x < ox then LESS
            else if x > ox then GREATER
            else if y < oy then LESS
            else if y > oy then GREATER
            else if ex < oex then LESS
            else if ex > oex then GREATER
            else if ey < oey then LESS
            else if ey > oey then GREATER
            else EQUAL
        in
          case dir of
            LESS =>
              NODE
                { left = insert (newItem, left)
                , right = right
                , x = ox
                , y = oy
                , ex = oex
                , ey = oey
                , data = oldData
                }
          | GREATER =>
              NODE
                { right = insert (newItem, right)
                , left = left
                , x = ox
                , y = oy
                , ex = oex
                , ey = oey
                , data = oldData
                }
          | EQUAL =>
              NODE
                { left = left
                , right = right
                , x = x
                , y = y
                , ex = ex
                , ey = ey
                , data = data
                }
        end

  fun toList (tree, acc) =
    case tree of
      NODE {x, y, ex, ey, data, left, right} =>
        let
          val acc = toList (right, acc)
          val acc = {x = x, y = y, ex = ex, ey = ey, data = data} :: acc
        in
          toList (left, acc)
        end
    | LEAF => acc
end

structure CollisionTree =
struct
  (* functions to check individual collisions *)
  fun isColliding (ix, iy, ifx, ify, cx, cy, cfx, cfy) =
    ix < cfx andalso ifx > cx andalso iy < cfy andalso ify > cy

  fun isCollidingPlus (ix, iy, iw, ih, cx, cy, cw, ch) =
    let
      val ifx = ix + iw
      val ify = iy + ih
      val cfx = cx + cw
      val cfy = cy + ch
    in
      isColliding (ix, iy, ifx, ify, cx, cy, cfx, cfy)
    end

  fun isCollidingItem (iX, iY, iW, iH, checkWith) =
    let val {x = cX, y = cY, w = cW, h = cH} = checkWith
    in isCollidingPlus (iX, iY, iW, iH, cX, cY, cW, cH)
    end

  fun visitTopLeft (iX, iY, qX, qY, size) =
    let
      val half = size div 2

      val qmx = qX + half
      val qmy = qY + half
    in
      iX >= qX andalso iX <= qmx andalso iY >= qY andalso iY <= qmy
    end

  fun visitTopRight (iX, iY, qX, qY, size) =
    let
      val half = size div 2

      val qmx = qX + half
      val qmy = qY + half - 1

      val qfx = qX + size
    in
      iX >= qmx andalso iX <= qfx andalso iY >= qY andalso iY <= qmy
    end

  fun visitBottomLeft (iX, iY, qX, qY, size) =
    let
      val half = size div 2

      val qmx = qX + half - 1
      val qmy = qY + half

      val qfy = qY + size
    in
      iX >= qX andalso iX <= qmx andalso iY >= qmy andalso iY <= qfy
    end

  fun visitBottomRight (iX, iY, qX, qY, size) =
    let
      val half = size div 2

      val qmx = qX + half
      val qmy = qY + half

      val qfx = qX + size
      val qfy = qY + size
    in
      iX >= qmx andalso iX <= qfx andalso iY >= qmy andalso iY <= qfy
    end

  (* types for tree *)
  datatype 'a tree =
    NODE of {tl: 'a tree, tr: 'a tree, bl: 'a tree, br: 'a tree}
  | LEAF of {x: int, y: int, ex: int, ey: int, data: 'a}

  type 'a t = {tree: 'a tree, size: int}

  local
    fun loopYAxis (x, y, eX, eY, yAxis, col) =
      if y > eY then
        true
      else
        let
          val newCol = Vector.sub (yAxis, y)
        in
          if col = newCol then loopYAxis (x, y + 1, eX, eY, yAxis, col)
          else false
        end

    fun loopColour (x, y, eX, eY, grid, col) =
      if x > eX then
        true
      else
        let
          val yAxis = Vector.sub (grid, x)
        in
          if loopYAxis (x, y, eX, eY, yAxis, col) then
            loopColour (x + 1, y, eX, eY, grid, col)
          else
            false
        end
  in
    fun quadHasSameColour (startX, startY, endX, endY, grid) =
      let
        val yAxis = Vector.sub (grid, startX)
        val col = Vector.sub (yAxis, startY)
      in
        loopColour (startX, startY, endX, endY, grid, col)
      end
  end

  (* tree creation/insertion/query functions *)
  fun build (x, y, size, grid) =
    if quadHasSameColour (x, y, x + size, y + size, grid) then
      let
        val yAxis = Vector.sub (grid, x)
        val data = Vector.sub (yAxis, y)
      in
        LEAF {x = x, y = y, ex = x + size, ey = y + size, data = data}
      end
    else
      let
        val halfSize = size div 2
        val tl = build (x, y, halfSize, grid)
        val tr = build (x + halfSize, y, halfSize, grid)
        val bl = build (x, y + halfSize, halfSize, grid)
        val br = build (x + halfSize, y + halfSize, halfSize, grid)
      in
        NODE {tl = tl, bl = bl, tr = tr, br = br}
      end

  fun foldWithDuplicates (f, tree, acc) =
    case tree of
      LEAF item => f (item, acc)
    | NODE {tl, tr, bl, br} =>
        let
          val acc = foldWithDuplicates (f, tl, acc)
          val acc = foldWithDuplicates (f, tr, acc)
          val acc = foldWithDuplicates (f, bl, acc)
        in
          foldWithDuplicates (f, br, acc)
        end

  fun insertItemIntoTree (item, acc) =
    if #data item <> 0 then
      BinTree.insert (item, acc)
    else acc

  fun toList qtree =
    let val tree = foldWithDuplicates (insertItemIntoTree, qtree, BinTree.empty)
    in BinTree.toList (tree, [])
    end

  local
    fun loop (windowWidth, windowHeight, squares, acc) =
      case squares of
        {x, y, ex, ey, data = _} :: tl =>
          let
            val x = Real32.fromInt x
            val y = Real32.fromInt y
            val ex = Real32.fromInt ex
            val ey = Real32.fromInt ey

            val startX = Ndc.fromPixelX (x, windowWidth, windowHeight)
            val endX = Ndc.fromPixelX (ex, windowWidth, windowHeight)
            val startY = Ndc.fromPixelY (y, windowWidth, windowHeight)
            val endY = Ndc.fromPixelY (ey, windowWidth, windowHeight)

            val vec = 
              Ndc.ltrbToVertexRgb (startX, startY, endX, endY, 0.0, 0.0, 0.0)
            val acc = vec :: acc
          in
            loop (windowWidth, windowHeight, tl, acc)
          end
      | [] => Vector.concat acc
  in
    fun toTriangles (windowWidth, windowHeight, squares, size) =
      let
        val qtree = build (0, 0, size, squares)
        val squares = toList qtree
        val msg = List.length squares
        val () = print (Int.toString msg ^ "\n")
      in
        loop (windowWidth, windowHeight, squares, [])
      end
  end
end