package simulation

import "core:math"
import cm "../common"

DerivX :: struct {
	x: [3]cm.iFP,
	v: [3]cm.iFP,
	a: [3]cm.iFP,
} 

AABB :: struct {
		solid: bool,
    min: [3]cm.iFP,
    max: [3]cm.iFP,
}

Ray :: struct {
    origin: [3]cm.iFP,
    heading: [3]cm.iFP,
}

KinematicUpdate :: proc(delta: cm.iFP, entities: ^[dynamic]Maybe(DerivX), boundingBoxes: ^[dynamic]Maybe(AABB), tree: ^[dynamic]OctreeNode) {
	using cm
	for e,i in entities^ {
		eNew, ok := e.?
		if !ok do continue

		startPos := eNew.x
		eNew.v += VecMul(eNew.a, delta)
		eNew.x += VecMul(eNew.v, delta)

		entities[i] = eNew

		bb, alsoOk := boundingBoxes[i].?
		if !alsoOk do continue

		sector1 := FindEnclosingNode(bb,startPos,tree)
		sector2 := FindEnclosingNode(bb,eNew.x,tree)

		if !RemoveEntityFromNode(i, sector1) {
			for _ in 0..<MAX_TREE_DEPTH {
				if sector1.parent == nil do break
				removed := RemoveEntityFromNode(i, sector1.parent)
				if removed do break
				sector1 = sector1.parent
			}
		}
		AddEntityToNode(i, sector2)
	}
}

ResetForces :: proc(entities: ^[dynamic]Maybe(DerivX)) {
	using cm
	for e,i in entities^ {
		eNew, ok := e.?
		if !ok do continue
		eNew.a = [3]iFP{0,0,0}

		entities[i] = eNew
	}
}

ApplyGravity :: proc(entities: ^[dynamic]Maybe(DerivX), gravity: ^[dynamic]Maybe(bool)) {
	using cm
	for e,i in entities^ {
		eNew, ok1 := e.?
		_, ok2 := gravity[i].?
		if !(ok1 && ok2) do continue
		eNew.a += [3]iFP{0,GRAVITY,0}

		entities[i] = eNew
	}
}

MoveCast :: proc(delta: cm.iFP, entities: ^[dynamic]Maybe(DerivX), boundingBoxes: ^[dynamic]Maybe(AABB), callbacks: ^[dynamic]Maybe(proc(e1: int, e2: int, delta: cm.iFP)) , tree: ^[dynamic]OctreeNode) {
	using cm
	for ent,i in entities^ {
		eNew, ok1 := ent.?
		function, ok2 := callbacks[i].?
		bb, ok3 := boundingBoxes[i].?
		if !(ok1 && ok2 && ok3 ) do continue

		start := eNew
		eNew.x = VecMul(eNew.a,Div(Mul(delta,delta),TWO_METER)) + VecMul(eNew.v,delta)

		if math.abs(DotProduct(start.x - eNew.x,start.x - eNew.x)) < TOLERENCE do continue

		sector1 := FindEnclosingNode(bb,start.x,tree)
		sector2 := FindEnclosingNode(bb,eNew.x,tree)
		sector12 : ^OctreeNode

		if sector1 != sector2 { 
			sector12 = FindCommonNode(sector1,sector2,tree)
			RemoveEntityFromNode(i,sector1)
			AddEntityToNode(i, sector12)
		} else do sector12 = sector1
		
		_,intersect := AABBCollision(bb, start.x, bb, eNew.x)

		if !intersect { // Ray 
			IterateTree(i, delta, RaySweep, sector12)
		}
		IterateTree(i, delta, function, sector12) // Bounding Box
	}
}


AABBCollision :: proc(b1: AABB, p1: [3]cm.iFP, b2: AABB, p2: [3]cm.iFP) -> (normal: [3]cm.iFP, collided: bool) {
		using cm
		collision := true
		penetration :iFP = INF
		n : [3]iFP

		delta := p1-p2
		extent := b1.max-b1.min + b2.max-b2.min

		for d,i in delta {
			if math.abs(d) >= extent[i] do collision = false
		}

		if !collision do return n, collision

		gb1 := AABB { max = b1.max + p1 , min = b1.min + p1 }
		gb2 := AABB { max = b2.max + p2 , min = b2.min + p2 }

		distances := [6]iFP{
			gb2.max[0]-gb1.min[0],
			gb1.max[0]-gb2.min[0],
			gb2.max[1]-gb1.min[1],
			gb1.max[1]-gb2.min[1],
			gb2.max[2]-gb1.min[2],
			gb1.max[2]-gb2.min[2],
		}

		for d,i in distances {
			if d < penetration {
				penetration = d
				n = BOX_NORMALS[i]
			}
		}

    return n, collision
}

RayAABBCollision :: proc(ray: Ray, bb: AABB, pos: [3]cm.iFP) -> (hit: bool, t_hit: cm.iFP, normal: [3]cm.iFP) {
		using cm
		bmin := bb.min+pos
		bmax := bb.max+pos
    tEnter := -INF
    tExit  :=  INF
    normal  = [3]iFP{0,0,0}

    for i in 0..<3 {
        if ray.heading[i] == 0 {
            if ray.origin[i] < bmin[i] || ray.origin[i] > bmax[i] {
                return false, 0, normal
            }
            continue
        }

        inv_d := Div(METER, ray.heading[i])

        t1 := Mul(bmin[i] - ray.origin[i], inv_d);
        t2 := Mul(bmax[i] - ray.origin[i], inv_d);

        faceNormal: [3]iFP = [3]iFP{0,0,0};

        if t1 > t2 {
            temp := t1; t1 = t2; t2 = temp
            faceNormal[i] = METER
        } else do faceNormal[i] = -METER

        if t1 > tEnter {
            tEnter = t1
            normal = faceNormal
        }

        if t2 < tExit do tExit = t2
        if tEnter > tExit do return false, 0, normal
    }

    if tExit < 0 do return false, 0, normal
    if tEnter < 0 do tEnter = 0

    return true, tEnter, normal
}

//Do a ray test, if it hits do e1's collsion callback
//Use delta as a parameter
RaySweep :: proc(e1: int, e2: int, delta: cm.iFP) {
	using cm
	x1, ok1 := entityDerivX[0][e1].?
	function, ok2 := entityCollisionCallback[0][e1].?
	x2, ok3 := entityDerivX[0][e2].?
	bb2, ok4 := entityAABB[0][e2].?
	if !(ok1 && ok2 && ok3 && ok4) do return

	x1New := VecMul(x1.a,Div(Mul(delta,delta),TWO_METER)) + VecMul(x1.v,delta) + x1.x
	ray := Ray{origin=x1.x, heading=x1New}

	collided, t, _ := RayAABBCollision(ray,bb2,x2.x)
	if !collided do return
	
	function(e1,e2,t)
}

RigidInelasticCollision :: proc(e1: int, e2: int, delta: cm.iFP) {
	using cm
	x1, ok1 := entityDerivX[0][e1].?
	bb1, ok2 := entityAABB[0][e1].?
	x2, ok3 := entityDerivX[0][e2].?
	bb2, ok4 := entityAABB[0][e2].?
	if !(ok1 && ok2 && ok3 && ok4) do return
	if !(bb1.solid && bb2.solid) do return

	x1New := VecMul(x1.a,Div(Mul(delta,delta),TWO_METER)) + VecMul(x1.v,delta) + x1.x

	normal, collsion := AABBCollision(bb1,x1New,bb2,x2.x)
	if !collsion do return
	
	wallImpulse := DotProduct(normal,x1.v)
	reactionForce := DotProduct(normal,x1.a)
	if wallImpulse < 0 do x1.v -= VecMul(normal,wallImpulse)
	if reactionForce < 0 do x1.a -= VecMul(normal,reactionForce)

	entityDerivX[0][e1] = x1
}