import std/sequtils

import ./core
import ./scene

const MAX_COLLISON_DETECTION_ITERATIONS = 10
const MAX_COLLISON_POINT_CALCULATION_ITERATIONS = 10

type
  HitBox* = ref object of Component
    transform*: Mat4
  HitSphere* = ref object of Component
    radius*: float32

func between(value, b1, b2: float32): bool =
  min(b1, b2) <= value and value <= max(b1, b2)

func contains*(hitbox: HitBox, x: Vec3f): bool =
  # from https://math.stackexchange.com/questions/1472049/check-if-a-point-is-inside-a-rectangular-shaped-area-3d
  let
    t = hitbox.entity.getModelTransform() * hitbox.transform
    P1 = t * newVec3f(0, 0, 0) # origin
    P2 = t * Z
    P4 = t * X
    P5 = t * Y
    u = (P1 - P4).cross(P1 - P5)
    v = (P1 - P2).cross(P1 - P5)
    w = (P1 - P2).cross(P1 - P4)
    uP1 = u.dot(P1)
    uP2 = u.dot(P2)
    vP1 = v.dot(P1)
    vP4 = v.dot(P4)
    wP1 = w.dot(P1)
    wP5 = w.dot(P5)
    ux = u.dot(x)
    vx = v.dot(x)
    wx = w.dot(x)

  result = ux.between(uP1, uP2) and vx.between(vP1, vP4) and wx.between(wP1, wP5)

# implementation of GJK, based on https://blog.winter.dev/2020/gjk-algorithm/

# most generic implementation of findFurthestPoint
# add other implementations of findFurthestPoint for other kind of geometry or optimization
# (will be selected depening on type of the first parameter)
func findFurthestPoint(points: openArray[Vec3f], direction: Vec3f): Vec3f =
  var maxDist = low(float32)
  for p in points:
    let dist = direction.dot(p)
    if dist > maxDist:
      maxDist = dist
      result = p

func findFurthestPoint(hitsphere: HitSphere, direction: Vec3f): Vec3f =
  let directionNormalizedToSphere = ((direction / direction.length) * hitsphere.radius)
  return hitsphere.entity.getModelTransform() * directionNormalizedToSphere

func findFurthestPoint(hitbox: HitBox, direction: Vec3f): Vec3f =
  let transform = hitbox.entity.getModelTransform() * hitbox.transform
  return findFurthestPoint(
    [
      transform * newVec3f(0, 0, 0),
      transform * X,
      transform * Y,
      transform * Z,
      transform * (X + Y),
      transform * (X + Z),
      transform * (Y + Z),
      transform * (X + Y + Z),
    ],
    direction
  )

func supportPoint[A, B](a: A, b: B, direction: Vec3f): Vec3f =
  a.findFurthestPoint(direction) - b.findFurthestPoint(-direction)

func sameDirection(direction: Vec3f, ao: Vec3f): bool =
  direction.dot(ao) > 0

func line(simplex: var seq[Vec3f], direction: var Vec3f): bool =
  let
    a = simplex[0]
    b = simplex[1]
    ab = b - a
    ao =   - a

  if sameDirection(ab, ao):
    direction = cross(cross(ab, ao), ab)
  else:
    simplex = @[a]
    direction = ao

  return false

func triangle(simplex: var seq[Vec3f], direction: var Vec3f): bool =
  let
    a = simplex[0]
    b = simplex[1]
    c = simplex[2]
    ab = b - a
    ac = c - a
    ao =   - a
    abc = ab.cross(ac)
 
  if sameDirection(abc.cross(ac), ao):
    if sameDirection(ac, ao):
      simplex = @[a, c]
      direction = ac.cross(ao).cross(ac)
    else:
      simplex = @[a, b]
      return line(simplex, direction)
  else:
    if (sameDirection(ab.cross(abc), ao)):
      simplex = @[a, b]
      return line(simplex, direction)
    else:
      if (sameDirection(abc, ao)):
        direction = abc
      else:
        simplex = @[ a, c, b]
        direction = -abc

  return false

func tetrahedron(simplex: var seq[Vec3f], direction: var Vec3f): bool =
  let
    a = simplex[0]
    b = simplex[1]
    c = simplex[2]
    d = simplex[3]
    ab = b - a
    ac = c - a
    ad = d - a
    ao =   - a
    abc = ab.cross(ac)
    acd = ac.cross(ad)
    adb = ad.cross(ab)
 
  if sameDirection(abc, ao):
    simplex = @[a, b, c]
    return triangle(simplex, direction)
  if sameDirection(acd, ao):
    simplex = @[a, c, d]
    return triangle(simplex, direction)
  if sameDirection(adb, ao):
    simplex = @[a, d, b]
    return triangle(simplex, direction)
 
  return true

func getFaceNormals(polytope: seq[Vec3f], faces: seq[int]): (seq[Vec4f], int) =
  var
    normals: seq[Vec4f]
    minTriangle = 0
    minDistance = high(float32)

  for i in countup(0, faces.len - 1, 3):
    let
      a = polytope[faces[i + 0]]
      b = polytope[faces[i + 1]]
      c = polytope[faces[i + 2]]

    var normal = (b - a).cross(c - a).normalized()
    var distance = normal.dot(a)

    if distance < 0:
      normal = normal * -1'f32
      distance = distance * -1'f32

    normals.add normal.toVec4(distance)

    if distance < minDistance:
      minTriangle = i div 3
      minDistance = distance

  return (normals, minTriangle)

func addIfUniqueEdge(edges: var seq[(int, int)], faces: seq[int], a: int, b: int) =
  let reverse = edges.find((faces[b], faces[a]))
  if (reverse >= 0):
    edges.delete(reverse)
  else:
    edges.add (faces[a], faces[b])

func nextSimplex(simplex: var seq[Vec3f], direction: var Vec3f): bool =
  case simplex.len
  of 2: simplex.line(direction)
  of 3: simplex.triangle(direction)
  of 4: simplex.tetrahedron(direction)
  else: raise newException(Exception, "Error in simplex")

func collisionPoint*[A, B](simplex: var seq[Vec3f], a: A, b: B): tuple[normal: Vec3f, penetrationDepth: float32] =
  var
    polytope = simplex
    faces = @[
      0, 1, 2,
      0, 3, 1,
      0, 2, 3,
      1, 3, 2
    ]
    (normals, minFace) = getFaceNormals(polytope, faces)
    minNormal: Vec3f
    minDistance = high(float32)
    iterCount = 0

  while minDistance == high(float32) and iterCount < MAX_COLLISON_POINT_CALCULATION_ITERATIONS:
    minNormal = normals[minFace].xyz
    minDistance = normals[minFace].w
    var
      support = supportPoint(a, b, minNormal)
      sDistance = minNormal.dot(support)
    
    if abs(sDistance - minDistance) > 0.001'f32:
      minDistance = high(float32)
      var uniqueEdges: seq[(int, int)]
      var i = 0
      while i < normals.len:
        if sameDirection(normals[i], support):
          var f = i * 3

          addIfUniqueEdge(uniqueEdges, faces, f + 0, f + 1)
          addIfUniqueEdge(uniqueEdges, faces, f + 1, f + 2)
          addIfUniqueEdge(uniqueEdges, faces, f + 2, f + 0)

          faces[f + 2] = faces.pop()
          faces[f + 1] = faces.pop()
          faces[f + 0] = faces.pop()

          normals[i] = normals.pop()

          dec i
        inc i

      var newFaces: seq[int]
      for (edgeIndex1, edgeIndex2) in uniqueEdges:
        newFaces.add edgeIndex1
        newFaces.add edgeIndex2
        newFaces.add polytope.len
       
      polytope.add support

      var (newNormals, newMinFace) = getFaceNormals(polytope, newFaces)
      if newNormals.len == 0:
        break

      var oldMinDistance = high(float32)
      for j in 0 ..< normals.len:
        if normals[j].w < oldMinDistance:
          oldMinDistance = normals[j].w
          minFace = j

      if (newNormals[newMinFace].w < oldMinDistance):
        minFace = newMinFace + normals.len

      for f in newFaces:
        faces.add f
      for n in newNormals:
        normals.add n
    inc iterCount

  result = (normal: minNormal, penetrationDepth: minDistance + 0.001'f32)

func intersects*[A, B](a: A, b: B): bool =
  var
    support = supportPoint(a, b, newVec3f(0.8153, -0.4239, 0.5786)) # just random initial vector
    simplex = newSeq[Vec3f]()
    direction = -support
    n = 0
  simplex.insert(support, 0)
  while n < MAX_COLLISON_DETECTION_ITERATIONS:
    support = supportPoint(a, b, direction)
    if support.dot(direction) <= 0:
        return false
    simplex.insert(support, 0)
    if nextSimplex(simplex, direction):
      return true
    # prevent numeric instability
    if direction == newVec3f(0, 0, 0):
      direction[0] = 0.0001
    inc n

func collision*[A, B](a: A, b: B): tuple[hasCollision: bool, normal: Vec3f, penetrationDepth: float32] =
  var
    support = supportPoint(a, b, newVec3f(0.8153, -0.4239, 0.5786)) # just random initial vector
    simplex = newSeq[Vec3f]()
    direction = -support
    n = 0
  simplex.insert(support, 0)
  while n < MAX_COLLISON_DETECTION_ITERATIONS:
    support = supportPoint(a, b, direction)
    if support.dot(direction) <= 0:
        return result
    simplex.insert(support, 0)
    if nextSimplex(simplex, direction):
      let (normal, depth) = collisionPoint(simplex, a, b)
      return (true, normal, depth)
    # prevent numeric instability
    if direction == newVec3f(0, 0, 0):
      direction[0] = 0.0001
    inc n

func calculateHitbox*(points: seq[Vec3f]): HitBox =
  var
    minX = high(float32)
    maxX = low(float32)
    minY = high(float32)
    maxY = low(float32)
    minZ = high(float32)
    maxZ = low(float32)

  for p in points:
    minX = min(minX, p.x)
    maxX = max(maxX, p.x)
    minY = min(minY, p.y)
    maxY = max(maxY, p.y)
    minZ = min(minZ, p.z)
    maxZ = max(maxz, p.z)

  let
    scaleX = (maxX - minX)
    scaleY = (maxY - minY)
    scaleZ = (maxZ - minZ)

  HitBox(transform: translate3d(minX, minY, minZ) * scale3d(scaleX, scaleY, scaleZ))

func calculateHitsphere*(points: seq[Vec3f]): HitSphere =
  result = HitSphere()
  for p in points:
    result.radius = max(result.radius, p.length)