func circle2circleQuery(p1, p2 vect.Vect, r1, r2 vect.Float, con *Contact) int {
	minDist := r1 + r2

	delta := vect.Sub(p2, p1)
	distSqr := delta.LengthSqr()

	if distSqr >= minDist*minDist {
		return 0
	}

	dist := vect.Float(math.Sqrt(float64(distSqr)))

	pDist := dist
	if dist == 0.0 {
		pDist = vect.Float(math.Inf(1))
	}

	pos := vect.Add(p1, vect.Mult(delta, 0.5+(r1-0.5*minDist)/pDist))

	norm := vect.Vect{1, 0}

	if dist != 0.0 {
		norm = vect.Mult(delta, 1.0/dist)
	}

	con.reset(pos, norm, dist-minDist, 0)

	return 1
}

func preStepGameObject(g *GameObject) {
	if g.Physics != nil && g.IsActive() && !g.Physics.Body.IsStatic() && g.Physics.Started() {
		pos := g.Transform().WorldPosition()
		angle := g.Transform().Angle() * RadianConst

		if g.Physics.Interpolate {
			//Interpolation check: if position/angle has been changed directly and not by the physics engine, change g.Physics.LastPosition/LastAngle
			if vect.Float(pos.X) != g.Physics.InterpolatedPosition.X || vect.Float(pos.Y) != g.Physics.InterpolatedPosition.Y {
				g.Physics.InterpolatedPosition = vect.Vect{vect.Float(pos.X), vect.Float(pos.Y)}
				g.Physics.Body.SetPosition(g.Physics.InterpolatedPosition)
			}
			if vect.Float(angle) != g.Physics.InterpolatedAngle {
				g.Physics.InterpolatedAngle = vect.Float(angle)
				g.Physics.Body.SetAngle(g.Physics.InterpolatedAngle)
			}
		} else {
			var pPos vect.Vect
			pPos.X, pPos.Y = vect.Float(pos.X), vect.Float(pos.Y)

			g.Physics.Body.SetAngle(vect.Float(angle))
			g.Physics.Body.SetPosition(pPos)
		}
		g.Physics.LastPosition = g.Physics.Body.Position()
		g.Physics.LastAngle = g.Physics.Body.Angle()
	}
}

func addBall() {
	x := rand.Intn(350-115) + 115
	ball := chipmunk.NewCircle(vect.Vector_Zero, float32(ballRadius))
	ball.SetElasticity(0.95)

	body := chipmunk.NewBody(vect.Float(ballMass), ball.Moment(float32(ballMass)))
	body.SetPosition(vect.Vect{vect.Float(x), 600.0})
	body.SetAngle(vect.Float(rand.Float32() * 2 * math.Pi))

	body.AddShape(ball)
	space.AddBody(body)
	balls = append(balls, ball)
}

func NewPhysics(static bool, space *chipmunk.Space) *Physics {
	var body *chipmunk.Body

	box := chipmunk.NewBox(vect.Vect{0, 0}, vect.Float(1), vect.Float(1))

	if static {
		body = chipmunk.NewBodyStatic()
	} else {
		body = chipmunk.NewBody(1, box.Moment(1))
	}

	p := &Physics{BaseComponent: NewComponent(), Space: space, Body: body, Box: box.GetAsBox(), Shape: box}
	body.AddShape(box)
	return p
}

func (p *Physics) Start() {
	//p.Interpolate = true
	pos := p.GameObject().Transform().WorldPosition()

	p.Body.SetAngle(vect.Float(p.GameObject().Transform().WorldRotation().Z) * RadianConst)
	p.Body.SetPosition(vect.Vect{vect.Float(pos.X), vect.Float(pos.Y)})
	p.LastPosition = p.Body.Position()
	p.LastAngle = p.Body.Angle()

	//if p.GameObject().Sprite != nil {
	//	p.GameObject().Sprite.UpdateShape()
	//	p.Body.UpdateShapes()
	//}

	p.Body.UpdateShapes()
	//log.Printf("3x=%f, y=%f", p.Body.Shapes[0].BB.Extents().X, p.Body.Shapes[0].BB.Extents().Y)
	p.Space.AddBody(p.Body)
}

func (arb *Arbiter) preStep(inv_dt, slop, bias vect.Float) {

	a := arb.ShapeA.Body
	b := arb.ShapeB.Body

	for _, con := range arb.Contacts {
		// Calculate the offsets.
		x, y := con.p.X, con.p.Y
		r1 := vect.Vect{x - a.p.X, y - a.p.Y}
		r2 := vect.Vect{x - b.p.X, y - b.p.Y}

		//con.Normal = vect.Vect{-1,0}

		// Calculate the mass normal and mass tangent.
		n := con.n
		rcn := (r1.X * n.Y) - (r1.Y * n.X)
		rcn = a.m_inv + (a.i_inv * rcn * rcn)

		rcn2 := (r2.X * n.Y) - (r2.Y * n.X)
		rcn2 = b.m_inv + (b.i_inv * rcn2 * rcn2)

		value := rcn + rcn2
		if value == 0.0 {
			fmt.Printf("Warning: Unsolvable collision or constraint.")
		}
		con.nMass = 1.0 / value

		n = vect.Perp(con.n)
		rcn = (r1.X * n.Y) - (r1.Y * n.X)
		rcn = a.m_inv + (a.i_inv * rcn * rcn)

		rcn2 = (r2.X * n.Y) - (r2.Y * n.X)
		rcn2 = b.m_inv + (b.i_inv * rcn2 * rcn2)

		value = rcn + rcn2
		if value == 0.0 {
			fmt.Printf("Warning: Unsolvable collision or constraint.")
		}
		con.tMass = 1.0 / value

		// Calculate the target bias velocity.
		//ds := con.dist + slop
		//if 0 > ds {
		//con.bias = -bias * inv_dt * (con.dist + slop)
		min := math.Min(0.0, float64(con.dist)+0.01)
		con.bias = -bias * inv_dt * vect.Float(min)
		//c->bias = -k_biasFactor * inv_dt * Min(0.0f, c->separation + 0.01);
		//} else {
		//	con.bias = 0
		//}
		con.jBias = 0.0
		con.bounce = vect.Dot(vect.Vect{(-r2.Y*b.w + b.v.X) - (-r1.Y*a.w + a.v.X), (r2.X*b.w + b.v.Y) - (r1.X*a.w + a.v.Y)}, con.n) * arb.e
		con.r1 = r1
		con.r2 = r2
	}
}

// Creates a new CircleShape with the given center and radius.
func NewCircle(pos vect.Vect, radius float32) *Shape {
	shape := newShape()
	circle := &CircleShape{
		Position: pos,
		Radius:   vect.Float(radius),
		Shape:    shape,
	}
	shape.ShapeClass = circle
	return shape
}

func k_tensor(a, b *Body, r1, r2 vect.Vect, k1, k2 *vect.Vect) {
	// calculate mass matrix
	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
	m_sum := a.m_inv + b.m_inv

	// start with I*m_sum
	k11 := vect.Float(m_sum)
	k12 := vect.Float(0)
	k21 := vect.Float(0)
	k22 := vect.Float(m_sum)

	// add the influence from r1
	a_i_inv := a.i_inv
	r1xsq := r1.X * r1.X * a_i_inv
	r1ysq := r1.Y * r1.Y * a_i_inv
	r1nxy := -r1.X * r1.Y * a_i_inv
	k11 += r1ysq
	k12 += r1nxy
	k21 += r1nxy
	k22 += r1xsq

	// add the influnce from r2
	b_i_inv := b.i_inv
	r2xsq := r2.X * r2.X * b_i_inv
	r2ysq := r2.Y * r2.Y * b_i_inv
	r2nxy := -r2.X * r2.Y * b_i_inv
	k11 += r2ysq
	k12 += r2nxy
	k21 += r2nxy
	k22 += r2xsq

	// invert
	determinant := (k11 * k22) - (k12 * k21)
	if determinant == 0 {
		panic("Unsolvable constraint.")
	}

	det_inv := 1.0 / determinant
	*k1 = vect.Vect{k22 * det_inv, -k12 * det_inv}
	*k2 = vect.Vect{-k21 * det_inv, k11 * det_inv}
}

func (poly *PolygonShape) Moment(mass float32) vect.Float {

	sum1 := vect.Float(0)
	sum2 := vect.Float(0)

	println("using bad Moment calculation")
	offset := vect.Vect{0, 0}

	for i := 0; i < poly.NumVerts; i++ {

		v1 := vect.Add(poly.Verts[i], offset)
		v2 := vect.Add(poly.Verts[(i+1)%poly.NumVerts], offset)

		a := vect.Cross(v2, v1)
		b := vect.Dot(v1, v1) + vect.Dot(v1, v2) + vect.Dot(v2, v2)

		sum1 += a * b
		sum2 += a
	}

	return (vect.Float(mass) * sum1) / (6.0 * sum2)
}

// step advances the physics engine and cleans up any balls that are off-screen
func step(dt float32) {
	space.Step(vect.Float(dt))

	for i := 0; i < len(balls); i++ {
		p := balls[i].Body.Position()
		if p.Y < -100 {
			space.RemoveBody(balls[i].Body)
			balls[i] = nil
			balls = append(balls[:i], balls[i+1:]...)
			i-- // consider same index again
		}
	}
}

// Calculates the transformed vertices and axes and the bounding box.
func (poly *PolygonShape) update(xf transform.Transform) AABB {
	//transform axes
	{
		src := poly.Axes
		dst := poly.TAxes

		for i := 0; i < poly.NumVerts; i++ {
			n := xf.RotateVect(src[i].N)
			dst[i].N = n
			dst[i].D = vect.Dot(xf.Position, n) + src[i].D
		}
		/*
			fmt.Println("")
			fmt.Println("Started Axes")
			fmt.Println(xf.Rotation, xf.Position)
			for i:=0;i<poly.NumVerts;i++ {
				fmt.Println(src[i], dst[i])
			}
		*/
	}
	//transform verts
	{
		inf := vect.Float(math.Inf(1))
		aabb := AABB{
			Lower: vect.Vect{inf, inf},
			Upper: vect.Vect{-inf, -inf},
		}

		src := poly.Verts
		dst := poly.TVerts

		for i := 0; i < poly.NumVerts; i++ {
			v := xf.TransformVect(src[i])

			dst[i] = v
			aabb.Lower.X = vect.FMin(aabb.Lower.X, v.X)
			aabb.Upper.X = vect.FMax(aabb.Upper.X, v.X)
			aabb.Lower.Y = vect.FMin(aabb.Lower.Y, v.Y)
			aabb.Upper.Y = vect.FMax(aabb.Upper.Y, v.Y)
		}

		/*
			fmt.Println("Verts")
			for i:=0;i<poly.NumVerts;i++ {
				fmt.Println(src[i], dst[i])
			}
		*/
		return aabb
	}
}

func NewSpace() (space *Space) {

	space = &Space{}
	space.Iterations = 20

	space.Gravity = vect.Vector_Zero

	space.damping = 1

	space.collisionSlop = 0.5
	space.collisionBias = vect.Float(math.Pow(1.0-0.1, 60))
	space.collisionPersistence = 3

	space.Constraints = make([]Constraint, 0)

	space.Bodies = make([]*Body, 0)
	space.deleteBodies = make([]*Body, 0)
	space.sleepingComponents = make([]*Body, 0)

	space.staticShapes = NewBBTree(nil)
	space.activeShapes = NewBBTree(space.staticShapes)
	space.cachedArbiters = make(map[HashPair]*Arbiter)
	space.Arbiters = make([]*Arbiter, 0)
	space.ArbiterBuffer = make([]*Arbiter, ArbiterBufferSize)

	for i := 0; i < len(space.ArbiterBuffer); i++ {
		space.ArbiterBuffer[i] = newArbiter()
	}

	space.ContactBuffer = make([][]*Contact, ContactBufferSize)

	for i := 0; i < len(space.ContactBuffer); i++ {
		var contacts []*Contact = make([]*Contact, MaxPoints)

		for i := 0; i < MaxPoints; i++ {
			contacts[i] = &Contact{}
		}
		space.ContactBuffer[i] = contacts
	}
	/*
		for i := 0; i < 8; i++ {
			go space.MultiThreadTest()
		}
	*/
	return
}

func postStepGameObject(g *GameObject) {
	if g.Physics != nil && g.IsActive() && !g.Physics.Body.IsStatic() && g.Physics.Started() {
		/*
			When parent changes his position/rotation it changes his children position/rotation too but the physics engine thinks its in different position
			so we need to check how much it changed and apply to the new position/rotation so we wont fuck up things too much.

			Note:If position/angle is changed in between preStep and postStep it will be overrided.
		*/
		if CorrectWrongPhysics {
			b := g.Physics.Body
			angle := float32(b.Angle())

			lAngle := float32(g.Physics.LastAngle)
			lAngle += angle - lAngle

			pos := b.Position()
			lPos := g.Physics.LastPosition
			lPos.X += (pos.X - lPos.X)
			lPos.Y += (pos.Y - lPos.Y)

			if g.Physics.Interpolate {
				g.Physics.InterpolatedAngle = vect.Float(lAngle)
				g.Physics.InterpolatedPosition = lPos
			}

			b.SetPosition(lPos)
			//b.SetAngle(g.Physics.InterpolatedAngle)

			//g.Transform().SetWorldRotationf(lAngle * DegreeConst)
			g.Transform().SetWorldPositionf(float32(pos.X), float32(pos.Y))
		} else {
			//b := g.Physics.Body
			//angle := b.Angle()
			//pos := b.Position()

			//if g.Physics.Interpolate {
			//	g.Physics.InterpolatedAngle = angle
			//	g.Physics.InterpolatedPosition = pos
			//}

			////g.Transform().SetWorldRotationf(float32(angle) * DegreeConst)
			//g.Transform().SetWorldPositionf(float32(pos.X), float32(pos.Y))
		}
	}
}

func NewPhysicsShapes(static bool, shapes []*chipmunk.Shape) *Physics {
	var body *chipmunk.Body
	if static {
		body = chipmunk.NewBodyStatic()
	} else {
		moment := vect.Float(0)
		for _, shape := range shapes {
			moment += shape.Moment(1)
		}
		body = chipmunk.NewBody(1, moment)
	}

	p := &Physics{BaseComponent: NewComponent(), Body: body, Box: nil, Shape: nil}
	for _, shape := range shapes {
		body.AddShape(shape)
	}
	return p
}

func interpolateGameObject(game *Game_t, g *GameObject) {
	if g.Physics != nil && g.Physics.Interpolate && g.IsActive() && !g.Physics.Body.IsStatic() && g.Physics.Started() {
		nextPos := g.Physics.Body.Position()
		currPos := g.Physics.LastPosition

		//nextAngle := g.Physics.Body.Angle()
		//currAngle := g.Physics.LastAngle

		alpha := vect.Float(game.fixedTime / stepTime)
		x := currPos.X + ((nextPos.X - currPos.X) * alpha)
		y := currPos.Y + ((nextPos.Y - currPos.Y) * alpha)
		//a := currAngle + ((nextAngle - currAngle) * alpha)
		g.Transform().SetWorldPositionf(float32(x), float32(y))
		//g.Transform().SetWorldRotationf(float32(a) * DegreeConst)

		//g.Physics.InterpolatedAngle = a
		g.Physics.InterpolatedPosition.X, g.Physics.InterpolatedPosition.Y = x, y
	}
}

func (tree *BBTree) GetBB(obj Indexable) AABB {
	v, ok := obj.Velocity()
	if ok {
		bb := obj.AABB()
		coef := vect.Float(0.1)

		l := bb.Lower.X
		b := bb.Lower.Y
		r := bb.Upper.X
		t := bb.Upper.Y

		x := (r - l) * coef
		y := (t - b) * coef

		v = vect.Mult(v, 0.1)

		return NewAABB(l+vect.FMin(-x, v.X), b+vect.FMin(-y, v.Y), r+vect.FMax(x, v.X), t+vect.FMax(y, v.Y))
	}

	return obj.AABB()
}

func (body *Body) SetAngularVelocity(w float32) {
	body.w = vect.Float(w)
}

func (body *Body) AddAngularVelocity(w float32) {
	body.w += vect.Float(w)
}

func (body *Body) SetTorque(t float32) {
	body.t = vect.Float(t)
}

func (body *Body) SetWBias(w float32) {
	body.w_bias = vect.Float(w)
}