import Foundation
import JolkEngine


class Collision
{
	struct Edge { let p: Vec2f, n: Vec2f, w: Float }

	enum Edge3D
	{
		case triangle(n: Vec3f, p: Vec3f, v: (Vec3f, Vec3f, Vec3f))
		case aabbFloor(n: Vec3f, p: Vec3f, w: Float, d: Float)
		case quad(p: (Vec3f, Vec3f, Vec3f, Vec3f), w: Winding)
	}
	var edge3d = [Edge3D]()

/*
0,-2      1,-2
     *--*
     |  |
0,-1 *  * 1,-1
     |  |
     *--*
0, 0      1, 0
 */
	

	init()
	{
		assert(Self.isRectangle([
			Vec3f(0.0, 0.0,  0.0),
			Vec3f(0.0, 0.0, -1.0),
			Vec3f(1.0, 0.0, -1.0),
			Vec3f(1.0, 0.0,  0.0),
		]))


		// CW
		assert(Self.isRectangle([
			Vec3f(0.0, 0.0,  0.0),
			Vec3f(0.0, 0.0, -1.0),
			Vec3f(0.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -1.0),
			Vec3f(1.0, 0.0,  0.0),
		]))
		assert(Self.isRectangle([
			Vec3f(0.0, 0.0, -1.0),
			Vec3f(0.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -1.0),
			Vec3f(1.0, 0.0,  0.0),
			Vec3f(0.0, 0.0,  0.0),
		]))
		assert(Self.isRectangle([
			Vec3f(0.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -1.0),
			Vec3f(1.0, 0.0,  0.0),
			Vec3f(0.0, 0.0,  0.0),
			Vec3f(0.0, 0.0, -1.0),
		]))
		assert(Self.isRectangle([
			Vec3f(1.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -1.0),
			Vec3f(1.0, 0.0,  0.0),
			Vec3f(0.0, 0.0,  0.0),
			Vec3f(0.0, 0.0, -1.0),
			Vec3f(0.0, 0.0, -2.0),
		]))
		assert(Self.isRectangle([
			Vec3f(1.0, 0.0, -1.0),
			Vec3f(1.0, 0.0,  0.0),
			Vec3f(0.0, 0.0,  0.0),
			Vec3f(0.0, 0.0, -1.0),
			Vec3f(0.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -2.0),
		]))
		assert(Self.isRectangle([
			Vec3f(1.0, 0.0,  0.0),
			Vec3f(0.0, 0.0,  0.0),
			Vec3f(0.0, 0.0, -1.0),
			Vec3f(0.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -2.0),
			Vec3f(1.0, 0.0, -1.0),
		]))

		// CCW
		assert(Self.isRectangle([
			Vec3f( 0.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0, -1.0),
			Vec3f( 0.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -1.0),
			Vec3f(-1.0, 0.0,  0.0),
		]))
		assert(Self.isRectangle([
			Vec3f( 0.0, 0.0, -1.0),
			Vec3f( 0.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -1.0),
			Vec3f(-1.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0,  0.0),
		]))
		assert(Self.isRectangle([
			Vec3f( 0.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -1.0),
			Vec3f(-1.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0, -1.0),
		]))
		assert(Self.isRectangle([
			Vec3f(-1.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -1.0),
			Vec3f(-1.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0, -1.0),
			Vec3f( 0.0, 0.0, -2.0),
		]))
		assert(Self.isRectangle([
			Vec3f(-1.0, 0.0, -1.0),
			Vec3f(-1.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0, -1.0),
			Vec3f( 0.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -2.0),
		]))
		assert(Self.isRectangle([
			Vec3f(-1.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0,  0.0),
			Vec3f( 0.0, 0.0, -1.0),
			Vec3f( 0.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -2.0),
			Vec3f(-1.0, 0.0, -1.0),
		]))
	}

	private static let epsilon: Float = 0.00001 //.ulpOfOne

	private static func isSimpleQuad(_ p: (Vec3f, Vec3f, Vec3f, Vec3f)) -> Bool
	{
		if abs(p.0.y - p.1.y) <= epsilon { return abs(p.2.y - p.3.y) <= epsilon }
		if abs(p.1.y - p.2.y) <= epsilon { return abs(p.3.y - p.0.y) <= epsilon }
		return false
	}

	private static func isRectangle(_ p: (Vec3f, Vec3f, Vec3f, Vec3f)) -> Bool
	{
		if	abs(p.0.x - p.1.x) <= epsilon &&
			abs(p.1.z - p.2.z) <= epsilon &&
			abs(p.2.x - p.3.x) <= epsilon &&
			abs(p.3.z - p.0.z) <= epsilon { return true }
		if	abs(p.0.z - p.1.z) <= epsilon &&
			abs(p.1.x - p.2.x) <= epsilon &&
			abs(p.2.z - p.3.z) <= epsilon &&
			abs(p.3.x - p.0.x) <= epsilon { return true }
		return false
	}

	private static func isRectangle(_ positions: [Vec3f]) -> Bool
	{
		var winding: Winding = .none
		var xdir: Float = 0.0, zdir: Float = 0.0
		let first = positions[0]
		var previous = first
		for p in positions[1...]
		{
			let (xdelta, zdelta) = (p.x - previous.x, p.z - previous.z)
			let (xzero, zzero) = (abs(xdelta) <= epsilon, abs(zdelta) <= epsilon)
			if !xzero && zzero
			{
				if xdir != 0.0 && xdelta.sign != xdir.sign { return false }
				if zdir != 0.0
				{
					switch winding
					{
					case .none: winding = xdelta.sign == zdir.sign ? .ccw : .cw
					case .cw:  if xdelta.sign == zdir.sign { return false }
					case .ccw: if xdelta.sign != zdir.sign { return false }
					}
				}
				(xdir, zdir) = (xdelta, 0.0)
			}
			else if xzero && !zzero
			{
				if zdir != 0.0 && zdelta.sign != zdir.sign { return false }
				if xdir != 0.0
				{
					switch winding
					{
					case .none: winding = zdelta.sign == xdir.sign ? .cw : .ccw
					case .cw:  if zdelta.sign != xdir.sign { return false }
					case .ccw: if zdelta.sign == xdir.sign { return false }
					}
				}
				(xdir, zdir) = (0.0, zdelta)
			}
			else if !xzero && !zzero { return false }
			previous = p
		}
		return abs(first.x - previous.x) <= epsilon || abs(first.z - previous.z) <= epsilon
	}

	private static func getQuadWinding(_ p: (Vec3f, Vec3f, Vec3f, Vec3f)) -> Winding
	{
		var area: Float = 0.0
		area += (p.1.x - p.0.x) * ((p.0.z + p.1.z) * 0.5)
		area += (p.2.x - p.1.x) * ((p.1.z + p.2.z) * 0.5)
		area += (p.3.x - p.2.x) * ((p.2.z + p.3.z) * 0.5)
		area += (p.0.x - p.3.x) * ((p.3.z + p.0.z) * 0.5)
		return area.sign == .plus ? .ccw : .cw  // z is towards us
	}

	static func quadSpaceFromCartesian(quad: (Vec3f, Vec3f, Vec3f, Vec3f), position: Vec3f) -> Vec2f
	{
		let p = (
			Vec2d(Double(quad.0.x), Double(quad.0.z)),
			Vec2d(Double(quad.1.x), Double(quad.1.z)),
			Vec2d(Double(quad.2.x), Double(quad.2.z)),
			Vec2d(Double(quad.3.x), Double(quad.3.z)))

		let xz = Vec2d(Double(position.x), Double(position.z))

		/*
		let old = {
			let a = xz.x - p.0.x
			let b = p.1.x - p.0.x
			let c = p.3.x - p.0.x
			let d = p.0.x - p.1.x + p.2.x - p.3.x
			let f = xz.y - p.0.y
			let g = p.1.y - p.0.y
			let h = p.3.y - p.0.y
			let j = p.0.y - p.1.y + p.2.y - p.3.y
			let v2 = -c * j - (-d * h)
			let v1 = a * j - c * g - (d * f - b * h)
			let v0 = a * g - b * f
			let vq = (-v1 + sqrt(v1 * v1 - 4.0 * v2 * v0)) / (2.0 * v2)
			let uq = (a - c * vq) / (b + d * vq)
			return Vec2f(Float(uq), Float(vq))
		}
		*/

		let a = xz - p.0
		let b = p.1 - p.0
		let c = p.3 - p.0
		let d = p.0 - p.1 + p.2 - p.3
		let v0 = a.cross(b), v2 = c.cross(-d)
		let v1 = a.x * d.y - b.y * c.x - (a.y * d.x - b.x * c.y)
		let vq = (-v1 + sqrt(v1 * v1 - 4.0 * v2 * v0)) / (2.0 * v2)
		let uq = (a.x - c.x * vq) / (b.x + d.x * vq)
		return Vec2f(Float(uq), Float(vq))
		//let oldUv = old()
		//if !oldUv.x.isNaN || !uv.x.isNaN { assert (oldUv.x == uv.x) }
		//if !oldUv.y.isNaN || !uv.y.isNaN { assert (oldUv.y == uv.y) }
		//return uv
	}

	enum Winding { case none, cw, ccw }

	func build(obj: ObjModel, collision: ObjModel.Object)
	{
		for face in collision.faces
		{
			switch face
			{
			case .triangle(let v1, let v2, let v3):
				let p = (obj.positions[v1.p], obj.positions[v2.p], obj.positions[v3.p])
				let n = (obj.normals[v1.n] + obj.normals[v2.n] + obj.normals[v3.n]).normalised
				if abs(n.y) < 0.25 { continue }
				edge3d.append(.triangle(n: n, p: (p.0 + p.1 + p.2) / 3.0, v: p))
			case .quad(let v1, let v2, let v3, let v4):
				let p = (obj.positions[v1.p], obj.positions[v2.p], obj.positions[v3.p], obj.positions[v4.p])
				let n = (obj.normals[v1.n] + obj.normals[v2.n] + obj.normals[v3.n] + obj.normals[v4.n]).normalised
				if Self.isSimpleQuad(p) && Self.isRectangle(p)
				{
					if abs(n.y) < 0.25 { continue }
					let left    = min(p.0.x, p.1.x, p.2.x, p.3.x)
					let right   = max(p.0.x, p.1.x, p.2.x, p.3.x)
					let back    = min(p.0.z, p.1.z, p.2.z, p.3.z)
					let forward = max(p.0.z, p.1.z, p.2.z, p.3.z)
					edge3d.append(.aabbFloor(n: n,
						p: (p.0 + p.1 + p.2 + p.3) / 4.0,
						w: (right - left) / 2.0,
						d: (forward - back) / 2.0))
				}
				else
				{
					if abs(n.y) < 0.25 { continue }
					edge3d.append(.quad(p: p, w: Self.getQuadWinding(p)))
					/*
					edge3d.append(.triangle(n: n, p: (p.0 + p.1 + p.2) / 3.0, v: (p.0, p.1, p.2)))
					edge3d.append(.triangle(n: n, p: (p.2 + p.3 + p.0) / 3.0, v: (p.2, p.3, p.0)))
					*/
				}
			case .ngon(let v):
				let p = v.map { obj.positions[$0.p] }
				let n = v.reduce(.zero) { $0 + obj.normals[$1.n] }.normalised
				if abs(n.y) < 0.25 { continue }
				if Self.isRectangle(p)
				{
					let left = p.map { $0.x }.min()!, right = p.map { $0.x }.max()!
					let bottom = p.map { $0.y }.min()!, top = p.map { $0.y }.max()!
					let back = p.map { $0.z }.min()!, forward = p.map { $0.z }.max()!
					let position = Vec3f(left + right, bottom + top, back + forward) / 2.0
					edge3d.append(.aabbFloor(n: n, p: position,
						w: (right - left) / 2.0,
						d: (forward - back) / 2.0))
				}
				else
				{
					let p0 = p[0]
					for i in 1..<(v.count-1)
					{
						let p1 = p[i], p2 = p[i + 1]
						edge3d.append(.triangle(n: n, p: (p0 + p1 + p2) / 3.0, v: (p0, p1, p2)))
					}
				}
			default:
				continue
			}
		}
	}

	func draw(_ render: Renderer, position: Vec3f)
	{
		var lines = [Line](
			repeating: .init(from: .init(), to: .init(), colour: .zero),
			count: edge3d.count * 6)
		var i: Int = 0
		for edge in edge3d
		{
			var o: Vec3f = .zero, n: Vec3f = .zero
			switch edge
			{
			case .triangle(let trin, let trip, let triv):
				o = trip
				n = trin

				let v0 = triv.1 - triv.0;
				let v1 = triv.2 - triv.0;
				let v2 = position - triv.0;
				let iden = 1.0 / (v0.x * v1.z - v1.x * v0.z);
				let v = (v2.x * v1.z - v1.x * v2.z) * iden;
				let w = (v0.x * v2.z - v2.x * v0.z) * iden;
				let colour = if v >= 0.0 && w >= 0.0 && v + w <= 1.0
				{ XnaColour.Red } else { XnaColour.GreenYellow }
				//let p = triv
				//let det = (p.1.x - p.0.x) * (p.2.z - p.0.z) - (p.1.z - p.0.z) * (p.2.x - p.0.x)
				//let colour: Colour = if
				//	det * (p.1.x - p.0.x) * (position.z - p.0.z) - (p.1.z - p.0.z) * (position.x - p.0.x) >= 0,
				//	det * (p.2.x - p.1.x) * (position.z - p.1.z) - (p.2.z - p.1.z) * (position.x - p.1.x) >= 0,
				//	det * (p.0.x - p.2.x) * (position.z - p.2.z) - (p.0.z - p.2.z) * (position.x - p.2.x) >= 0
				/*
				let side = { (v1: Vec3f, v2: Vec3f, p: Vec3f) in
					(v2.z - v1.z) * (p.x - v1.x) + (v2.x + v1.x) * (position.z - v1.z) }
				let colour = if
					side(triv.0, triv.1, position) >= 0,
					side(triv.1, triv.2, position) >= 0,
					side(triv.2, triv.0, position) >= 0
				*/

				lines[i + 0] = Line(from: triv.0, to: triv.1, colour: colour)
				lines[i + 1] = Line(from: triv.1, to: triv.2, colour: colour)
				lines[i + 2] = Line(from: triv.2, to: triv.0, colour: colour)
				i += 3
			case .aabbFloor(let floorn, let floorp, let floorw, let floord):
				o = floorp
				n = floorn

				let n2 = Vec2f(n.x, n.z) / n.y
				let z0 = Vec2f(-floorw,  floord).dot(n2)
				let z1 = Vec2f(-floorw, -floord).dot(n2)
				let z2 = Vec2f( floorw, -floord).dot(n2)
				let z3 = Vec2f( floorw,  floord).dot(n2)

				let c0 = floorp + Vec3f( floorw, z0, -floord)
				let c1 = floorp + Vec3f( floorw, z1,  floord)
				let c2 = floorp + Vec3f(-floorw, z2,  floord)
				let c3 = floorp + Vec3f(-floorw, z3, -floord)

				lines[i + 0] = Line(from: c0, to: c1, colour: XnaColour.GreenYellow)
				lines[i + 1] = Line(from: c1, to: c2, colour: XnaColour.GreenYellow)
				lines[i + 2] = Line(from: c2, to: c3, colour: XnaColour.GreenYellow)
				lines[i + 3] = Line(from: c3, to: c0, colour: XnaColour.GreenYellow)
				i += 4
			case .quad(let verts, let winding):
				/*
				let p = (
					verts.0 + (verts.1 - verts.0) * position.x,
					verts.3 + (verts.2 - verts.3) * position.x,
					verts.0 + (verts.3 - verts.0) * position.z,
					verts.1 + (verts.2 - verts.1) * position.z)

				let xdiff = Vec2f(p.0.x - p.1.x, p.2.x - p.3.x)
				let ydiff = Vec2f(p.0.z - p.1.z, p.2.z - p.3.z)
				let div = xdiff.cross(ydiff)
				guard div != 0.0 else { break }

				let d = Vec2f(
					Vec2f(p.0.x, p.0.z).cross(Vec2f(p.1.x, p.1.z)),
					Vec2f(p.2.x, p.2.z).cross(Vec2f(p.3.x, p.3.z)))
				let pos = Vec2f(d.cross(xdiff), d.cross(ydiff)) / div

				o = Vec3f(pos.x, 0.0, pos.y)
				n = .up
				*/

				//n = winding == .ccw ? .up : .down
				o = verts.0.lerp(verts.3, 0.5).lerp(
					verts.2.lerp(verts.1, 0.5), 0.5)

				/*
				let xy = Vec2d(Double(position.x), Double(position.z))
				let a = xy.x - p.0.x
				let b = p.1.x - p.0.x
				let c = p.3.x - p.0.x
				let d = p.0.x - p.1.x + p.2.x - p.3.x
				let f = xy.y - p.0.y
				let g = p.1.y - p.0.y
				let h = p.3.y - p.0.y
				let j = p.0.y - p.1.y + p.2.y - p.3.y
				let v2 = -c * j - (-d * h)
				let v1 = a * j - c * g - (d * f - b * h)
				let v0 = a * g - b * f
				let vq = (-v1 + sqrt(v1 * v1 - 4.0 * v2 * v0)) / (2.0 * v2)
				let uq = (a - c * vq) / (b + d * vq)
				let uv = Vec2d(uq, vq)
				*/

				var colour = XnaColour.GreenYellow

/*
p1          p2
   *------*
   |      |
   |      |
   *------*
p0          p3
 */

				let p = winding == .ccw ? (verts.3, verts.2, verts.1, verts.0) : (verts.0, verts.1, verts.2, verts.3)
				let vn = (
					(verts.1 - verts.0).cross(verts.3 - verts.0),
					(verts.2 - verts.1).cross(verts.0 - verts.1),
					(verts.3 - verts.2).cross(verts.1 - verts.2),
					(verts.0 - verts.3).cross(verts.2 - verts.3)
				)
				n = (vn.0 + vn.1 + vn.2 + vn.3).normalised

				let uv = Self.quadSpaceFromCartesian(quad: p, position: position)
				if uv.x >= 0.0 && uv.x <= 1.0 && uv.y >= 0.0 && uv.y <= 1.0
				//if uv.x >= -1.0 && uv.x <= 1.0 && uv.y >= -1.0 && uv.y <= 1.0
				{
					var pp = p.0
					pp += (p.1 - p.0) * uv.x
					pp += (p.3 - p.0) * uv.y
					pp += (p.0 - p.1 + p.2 - p.3) * uv.x * uv.y
					lines[i] = Line(from: o, to: pp, colour: XnaColour.Aquamarine)
					i += 1
					colour = XnaColour.BurlyWood
					o = pp
					n = vn.0.lerp(vn.3, uv.x).lerp(
						vn.2.lerp(vn.1, uv.x), winding == .cw ? uv.y : 1.0 - uv.y).normalised
				}

				lines[i + 0] = Line(from: verts.0, to: verts.1, colour: colour)
				lines[i + 1] = Line(from: verts.1, to: verts.2, colour: colour)
				lines[i + 2] = Line(from: verts.2, to: verts.3, colour: colour)
				lines[i + 3] = Line(from: verts.3, to: verts.0, colour: colour)
				i += 4
			}
			let p = position
			let a = p.dot(n) - o.dot(n)
			let pissy = p - n * a

			//let d = colin.position - edge.p
			//let pissy = edge.p + d.cross(edge.n)

			/*
			if  a > -1.0 && a <= 0.0
			{
				lines[i + 0] = Line(from: o, to: o + n * max(0, a), colour: XnaColour.Magenta)
				lines[i + 1] = Line(from: pissy + .up, to: o, colour: XnaColour.Red)
				i += 2
			}
			*/
				lines[i + 0] = Line(from: o, to: o + n * 0.2, colour: XnaColour.Magenta)
			i += 1
			//lines[i + 4] = Line(from: pissy + Vec3f(-1, 0, 0) * 0.1, to: pissy + Vec3f(1, 0, 0) * 0.1, colour: XnaColour.Red)
			//lines[i + 5] = Line(from: pissy + Vec3f( 0,-1, 0) * 0.1, to: pissy + Vec3f(0, 1, 0) * 0.1, colour: XnaColour.Red)
			//lines[i + 6] = Line(from: pissy + Vec3f( 0, 0,-1) * 0.1, to: pissy + Vec3f(0, 0, 1) * 0.1, colour: XnaColour.Red)
			
		}
		render.drawGizmos(lines: lines)
	}
}