import SDL2
import simd
import OpenGL.GL
import JolkEngine


class Colin: Actor
{
	var position: Vec3f { _pos }
	
	var transform: Mat4f
	{
		//.rotate(yaw: angle.x, pitch: angle.y, roll: sin(time)) *
		//.scale(Vec3f(1.0 + 0.25 * cos(time), 1.0 + 0.25 * sin(time), 1.0)) *
		.rotate(yawPitch: angle) * .translate(-position - Vec3f(0, 1, 0))
	}
	
	var angle: Vec2f { return Vec2f(ofsAngle.x + _angle, ofsAngle.y) }
	var fov: Float { return _fov }

	private var time: Float = 0.0
	//private var jumpVel: Float = 0.0
	private var velocity: Vec3f = .zero
	private var _pos: Vec3f     = .zero
	private var _pos2D: Vec2f { get { Vec2f(_pos.x, _pos.z) } set(new) { _pos.x = new.x; _pos.z = new.y } }
	private var _angle: Float   = 0.0
	private var ofsAngle: Vec2f = .zero
	private var _fov: Float     = 60.0

	private var nutted      = false
	private var colinMode   = false
	private var backPressed = false

	private let colinWidth: Float = 0.2

	private func move2D(new: Vec2f, edges: [Collision.Edge])
	{
		let velocity = new - _pos2D
		var lastPos = _pos2D
		_pos2D = new

		var collided = -1
		jolk: while (collided != 0)
		{
			let scabidabadoo = 10
			for edge in edges
			{
				let diff = _pos2D - lastPos
				if simd_dot(edge.n, diff) > 0 && simd_dot(edge.n, velocity) > 0 { continue }

				let deltaPos = _pos2D - edge.p
				let something = deltaPos.cross(edge.n)
				if abs(something) * 2.0 < edge.w
				{
					let dot = simd_dot(edge.n, deltaPos)
					if dot > 0 && dot < colinWidth
					{
						lastPos = _pos2D
						_pos2D += edge.n * (colinWidth - dot)
						if collided < 0 { collided = scabidabadoo };
					}
				}
				else
				{
					let vec = (deltaPos - Vec2f(edge.n.y, -edge.n.x) * Float(signOf: something, magnitudeOf: edge.w * 0.5))
					let distance2 = vec.len2
					let epsilon: Float = 0.001
					if distance2 < (colinWidth - epsilon) * (colinWidth - epsilon)
					{
						let distance = distance2.squareRoot()
						lastPos = _pos2D
						_pos2D += (vec / distance) * (colinWidth - epsilon - distance)
						if collided < 0 { collided = scabidabadoo };
					}
				}
			}
			collided -= 1;
			if collided <= 0 { break jolk }
		}
	}

	private func response3D(plainPos o: Vec3f, plainNorm n: Vec3f)
	{
		let height: Float = 1.12
		//let diff = _pos - lastPos

		//let p = n.y > 1.0 ? _pos : (_pos + 2.0)
		let ofs: Vec3f = n.y.sign == .plus ? .zero : .up * height
		//let p = if n.y.sign == .plus { _pos } else { _pos + 1.0 }
		let p = _pos + ofs
		let a = p.dot(n) - o.dot(n)

		if a > height * -0.5 && a < 0.0
		{
			_pos -= n * a
			velocity -= velocity.project(n)
		}
	}

	private func move3D(_ deltaTime: Float, world: Collision)
	{
		var lastPos = _pos
		_pos += velocity * deltaTime

		for edge in world.edge3d
		{
			switch edge
			{
			case .aabbFloor(let normal, let origin, let width, let depth):
				//let left    = min(edge.v0.x, edge.v1.x, edge.v2.x)
				//let right   = max(edge.v0.x, edge.v1.x, edge.v2.x)
				//let back    = min(edge.v0.z, edge.v1.z, edge.v2.z)
				//let forward = max(edge.v0.z, edge.v1.z, edge.v2.z)

				let left = origin.x - width, right = origin.x + width
				let back = origin.z - depth, forward = origin.z + depth

				if _pos.x < left || _pos.x > right   { continue }
				if _pos.z < back || _pos.z > forward { continue }

				response3D(plainPos: origin, plainNorm: normal)

			case .triangle(let normal, let origin, let triangle):
				if triangle.inside(point: _pos)
				{
					response3D(plainPos: origin, plainNorm: normal)
				}

			case .flatQuad(let quad, let normal):
				if quad.inside(point: _pos)
				{
					response3D(plainPos: quad.a, plainNorm: normal)
				}

			case .quad(let quad, let winding):
				if !quad.inside(point: _pos) { continue }
				let d = winding == .ccw ? quad.reverse : quad
				let uv = d.project(point: _pos)
				//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
				let p = quad.a.lerp(quad.b, uv.x).lerp(
						quad.d.lerp(quad.c, uv.x), winding == .cw ? uv.y : 1.0 - uv.y)
				let vn = quad.normals
				let n = vn.0.lerp(vn.1, uv.x).lerp(
						vn.3.lerp(vn.2, uv.x), winding == .cw ? uv.y : 1.0 - uv.y).normalised

				response3D(plainPos: Vec3f(_pos.x, p.y, _pos.z), plainNorm: n)
				/*
				let p = (
					verts.0 + (verts.1 - verts.0) * _pos.x,
					verts.3 + (verts.2 - verts.3) * _pos.x,
					verts.0 + (verts.3 - verts.0) * _pos.z,
					verts.1 + (verts.2 - verts.1) * _pos.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
				*/
			default: break
			}
		}
	}

	func update(deltaTime: Float, world: Collision)
	{
		time += deltaTime

		let axisRescale = 1.0 / Float(INT16_MAX)
		let getAxis = { (axis: SDL_GameControllerAxis) in
			Float(SDL_GameControllerGetAxis(sdlPad, axis)) * axisRescale }
		let getButton = { (btn: SDL_GameControllerButton) in
			SDL_GameControllerGetButton(sdlPad, btn) == SDL_PRESSED }

		let stick = Vec2f(
			getAxis(SDL_CONTROLLER_AXIS_LEFTX),
			getAxis(SDL_CONTROLLER_AXIS_LEFTY))
				.cardinalDeadzone(min: 0.1, max: 1)

		let turnSpeed = Float.pi * 2 * 0.1
		if Input.instance.keyDown(.left)
		{
			_angle -= turnSpeed * deltaTime
		}
		if Input.instance.keyDown(.right)
		{
			_angle += turnSpeed * deltaTime
		}
		if stick != .zero
		{
			_angle += stick.x * 2.0 * turnSpeed * deltaTime
		}

		var moveVec: Vec2f = .zero

		let speed: Float = 2
		let forward = Vec2f(sin(self._angle), -cos(self._angle))
		if Input.instance.keyDown(.up)
		{
			moveVec += forward * speed
		}
		if Input.instance.keyDown(.down)
		{
			moveVec -= forward * speed
		}
		if stick != .zero
		{
			moveVec -= forward * stick.y * speed
		}

		if Input.instance.keyDown(.c)
		{
			colinMode = !colinMode
		}

		let lookCone = Float.pi / 2.0
		let dst = Vec2f(
			getAxis(SDL_CONTROLLER_AXIS_RIGHTX),
			getAxis(SDL_CONTROLLER_AXIS_RIGHTY))
				.radialDeadzone(min: 0.1, max: 1) * lookCone
		ofsAngle = ofsAngle.lerp(dst, 16 * deltaTime)

		let targetFov = Float.lerp(60, 20, getAxis(SDL_CONTROLLER_AXIS_TRIGGERRIGHT))
		_fov = Float.lerp(_fov, targetFov, 20 * deltaTime)

		let right = Vec2f(cos(self._angle), sin(self._angle))
		if getButton(SDL_CONTROLLER_BUTTON_LEFTSHOULDER)
		{
			moveVec -= right * speed
		}
		if getButton(SDL_CONTROLLER_BUTTON_RIGHTSHOULDER)
		{
			moveVec += right * speed
		}

		let back = getButton(SDL_CONTROLLER_BUTTON_BACK)
		if !backPressed && back { colinMode = !colinMode }
		backPressed = back

		if getButton(SDL_CONTROLLER_BUTTON_A) || Input.instance.keyDown(.n)
		{
			// play nut.wav
			nutted = true
		}
		else { nutted = false }

		//move2D(new: _pos2D + velocity, edges: edges)

		if Input.instance.keyDown(.z) || getButton(SDL_CONTROLLER_BUTTON_B) { velocity.y = 2.0 }
		//_pos.y += jumpVel * deltaTime
		//if _pos.y > 0.0 { jumpVel -= 5.4 * deltaTime }
		velocity.y -= 5.4 * deltaTime
		velocity.x = moveVec.x
		velocity.z = moveVec.y
		move3D(deltaTime, world: world)
		//else if _pos.y < 0.0
		//{
		//	jumpVel = max(jumpVel, 0.0)
		//	_pos.y = 0.0
		//}
	}
}