
# Invivo AI challenge simulation environment
# Copyright 2009 Simon Funk
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.

"""Units are meters, kilograms, seconds, and radians.

X+ is right, Y+ is up.
"""
import math

from Utils import ObjectWithProperties

class World(object):
	"""A World is a finite 2d space containing some number of bodies."""

	def __init__(self, bounds, bodies, joints=(), gravity=-9.8, fps=60., bgcolor=(0,0,0)):

		"""Bounds is a tuple (min, max) of points, i.e., ((xmin, ymin), (xmax, ymax))
		Bodies is a list or tuple of bodies in the world.
		Joints is a list of tuple of joints in the world.
		Fps is frames per second for the simulation.
		Bgcolor is background color (0..255)*3
		"""
		self.gravity = gravity
		self.bounds  = bounds
		self.bodies  = list(bodies)
		self.joints  = list(joints)

		self.fps      = fps
		self.timestep = 1./fps
		self.bgcolor  = bgcolor

		self.time = 0.		# Current sim time.
		self.stop = False	# Set to true when it's time to exit...

		# For now, add in boundary bodies automatically:
		self.bodies.append(FixedBox(((bounds[0][0]-0.5, bounds[0][1]-0.5),(bounds[1][0]+0.5, bounds[0][1]    )), color=(0, 255, 0), name="Ground"))
		self.bodies.append(FixedBox(((bounds[0][0]-0.5, bounds[0][1]-0.5),(bounds[0][0]    , bounds[1][1]+0.5)), name="Left Wall"))
		self.bodies.append(FixedBox(((bounds[0][0]-0.5, bounds[1][1]    ),(bounds[1][0]+0.5, bounds[1][1]+0.5)), name="Sky"))
		self.bodies.append(FixedBox(((bounds[1][0]    , bounds[0][1]-0.5),(bounds[1][0]+0.5, bounds[1][1]+0.5)), name="Right Wall"))

		self.watchers = []

	def watch(self, func):
		"""Use this is you want to be notified when the World has changed (such
		  as after a simulation step, user edit, or other change).
		Func will only be called with a complete and consistent state, such as
		  at the end of a simulation step, never after a partial change.
		Func is called as func(self) aka func(world)
		"""
		self.watchers.append(func)

	def changed(self, time):
		self.time = time
		for func in self.watchers:
			func(self)

class Body(object):

	"""A Body is a collection of shapes (for now just convex polygons and full circles) welded together.
	"""

	def __init__(self, position, angle, shapes, dynamic=True, CCD=False, velocity=(0., 0.), rotating=0., name=None, contacts=False):
		"""Position is an (x, y) tuple specifying where the body starts in the world.
		Shapes is either a single Shape, or a list of shapes, describing the body.
		If Dynamic is False, this body is fixed in space and will never move.
		CCD = continuous collision detection.  This is slower, but is required
		  to keep fast-moving objects from tunnelling through eachother.
		If contacts is True, then self.contacts will be maintained as a list of Contact
		  objects, updated each simulation step.  (The list will be empty after any
		  step in which this body touches nothing.)
		"""

		if not isinstance(shapes, (list, tuple)):
			shapes = [shapes]

		#
		# These generally speaking do not change:
		#
		self.name    = name			# None if undefined.
		self.shapes  = shapes
		self.dynamic = dynamic
		self.ccd     = CCD

		#
		# These all update with time (by the physical sim -- do not change manually):
		#
		self.position = position
		self.angle    = angle
		self.velocity = velocity
		self.rotating = rotating # Angular velocity

		if contacts:
			self.contacts = []
		else:
			self.contacts = None

		#
		# These are updated when angle is:
		#
		self.cosa = 1.	# cos(self.angle)
		self.sina = 0.	# sin(self.angle)

	def move(self, position, angle):
		self.position = position
		self.angle    = angle
		self.cosa     = math.cos(angle)
		self.sina     = math.sin(angle)

	def toWorld(self, pt):
		"""Maps pt from local coordinates to world coordinates."""
		return (self.position[0] + pt[0]*self.cosa - pt[1]*self.sina,
		        self.position[1] + pt[1]*self.cosa + pt[0]*self.sina)

	def toLocal(self, pt):
		"""Maps pt from world coordinates to local coordinates."""
		x = pt[0] - self.position[0]
		y = pt[1] - self.position[1]
		return (x*self.cosa + y*self.sina, y*self.cosa - x*self.sina)

class Shape(ObjectWithProperties):
	"""All coordinates are (x, y) tuples in the body coordinate space.
	A single Shape may be used in multiple bodies provided it suffices to
	  be at the same relative coordinates in each of them.
	"""
	#
	# Default property values:
	#
	color       = (255,255,255)
	friction    = 0.2
	restitution = 0.0		# How bouncy is it?  1.0 = no energy loss.
	density     = 1.
	layers      = 0xffff	# Mask of which of 16 layers the shape exists in (for collisions)

class Circle(Shape):

	marker = True	# Draw a line so you can see spin?

	def __init__(self, center, radius, **properties):
		Shape.__init__(self, properties)
		self.center = center
		self.radius = radius

	def worldPos(self, body):
		return body.toWorld(self.center)

class Poly(Shape):

	def __init__(self, points, **properties):
		Shape.__init__(self, properties)

		assert len(points) <= 8	# Limitation of box2d ; can be extended with effort.

		#
		# Make sure the poly is convex, counter-clockwise, and nowhere co-linear:
		#
		norms = normsOf(edgesOf(points))
		for a, b in pairsOf(norms):
			assert a*b > 0.
		#
		# (We'll reverse it if it was specified in clockwise order...)
		#
		if sum(norms) < 0.:
			points = list(reversed(points))

		self.points = points

	def worldPoints(self, body):
		return [body.toWorld(pt) for pt in self.points]

class Box(Poly):

	def __init__(self, (min, max), **properties):
		Poly.__init__(self, [min, (max[0], min[1]), max, (min[0], max[1])], **properties)

def OnePoly(points, **properties):
	a, b = Poly.splitProps(properties)
	return Body((0., 0.), 0., Poly(points, **a), **b)

def FixedPoly(points, **properties):
	return OnePoly(points, dynamic=False, **properties)

def FixedBox(bounds, **properties):
	a, b = Box.splitProps(properties)
	return Body((0.,0.), 0., Box(bounds, **a), dynamic=False, **b)


class Ball(Body):
	"""A Ball is a dynamic (moving) circle of the given radius.
	Continuous collision detection is enabled since balls of all things are likely to need it...
	"""
	def __init__(self, pos, radius, **properties):

		a, b = Circle.splitProps(properties)
		self.circle = Circle((0., 0.), radius, **a)
		Body.__init__(self, pos, 0., self.circle, CCD=True, **b)

	def setColor(self, color):
		self.circle.color = color


class Bar(Body):
	"""A Bar is a box stretched between points a and b, with circle end-caps.
	The width of the bar is 2*radius, and the end-caps extend beyond a and b by radius.
	The bar is dynamic.
	"""
	def __init__(self, a, b, radius, **properties):

		sprops, bprops = Shape.splitProps(properties)	# Shape props, body props

		cen = pavg (a, b)
		len = pdist(a, b) * 0.5		# Length from center to end of each arm.
		ang = pangle(psub(b, a))	# Angle of the arm away from horizontal.

		acap = Circle(( len, 0.), radius, marker=False, **sprops)
		bcap = Circle((-len, 0.), radius, marker=False, **sprops)
		bar  = Box(((-len, -radius), (len, radius)), **sprops)

		Body.__init__(self, cen, ang, [acap, bcap, bar], **bprops)


class Joint(ObjectWithProperties):
	pass

class Pivot(Joint):

	#
	# Default properties:
	#
	torque = 0.

	limitAngle = True
	minAngle   = -math.pi
	maxAngle   =  math.pi
	friction   = 0.					# Added directly to torque in opposite direction of motion (or of torque if still)
	#maxSpeed   = (math.pi*2.) * 2.	# Radians per second (default = 2 revolutions per second)

	def __init__(self, body1, body2, position, **properties):
		Joint.__init__(self, properties)

		self.body1    = body1
		self.body2    = body2
		self.position = position

		self.angle = 0. # Current joint angle.
		self.speed = 0.	# Current angular speed

class Contact(object):
	__slots__ = ('body', 'impulse', 'worldpos')

	def __init__(self, body, impulse, worldpos):

		self.body     = body		# The other body we hit.
		self.impulse  = impulse		# The impulse force repelling us (how hard we hit...)
		self.worldpos = worldpos	# The location of the contact, in world coordinates.

	def __str__(self):
		return "Contact(%s,%s,%s)"%(self.body.name, self.impulse, self.worldpos)

#
# === Misc utils ===
#

def pairsOf(seq):
	return zip(seq, seq[1:] + [seq[0]])

def edgesOf(points):
	"""Returns a list of deltas from each point to the next, and back to the first."""
	return [psub(b, a) for a, b in pairsOf(points)]

def normsOf(edges):
	return [pcross(a, b) for a, b in pairsOf(edges)]

# --- "point" math ---

def padd(a, b):
	return (a[0] + b[0], a[1] + b[1])

def psub(a, b):
	return (a[0] - b[0], a[1] - b[1])

def pscale(a, s):
	return (a[0]*s, a[1]*s)

def pavg(a, b):
	return pscale(padd(a, b), 0.5)

def pdot(a, b):
	return a[0]*b[0] + a[1]*b[1]

def plen2(a):
	return pdot(a, a)

def plen(a):
	return math.sqrt(plen2(a))

def pdist(a, b):
	return plen(psub(a, b))

def pangle(a):
	return math.atan2(a[1], a[0])

def pcross(a, b):
	"Positive if b is counter-clockwise of a."
	return a[0]*b[1] - a[1]*b[0]

# --- color math ---

def icolor((r, g, b), scale=1.):
	"""Converts a 3-tuple of 0..1 floats to a 3-tuple of 0..255 ints,
	with optional scaling (brightness).
	"""
	r *= scale
	g *= scale
	b *= scale
	r = max(0., min(1., r))
	g = max(0., min(1., g))
	b = max(0., min(1., b))
	return (int(r*255.99), int(g*255.99), int(b*255.99))

def cadd(a, b):
	return (a[0] + b[0], a[1] + b[1], a[2] + b[2])

def csub(a, b):
	return (a[0] - b[0], a[1] - b[1], a[2] - b[2])

def cscale(a, s):
	return (a[0]*s, a[1]*s, a[2]*s)

def cmix(a, b, frac):
	return cadd(cscale(a, frac), cscale(b, 1.-frac))

if __name__ == "__main__":

	a = Poly([(0, 0), (10, 0), (5, 10)])
	b = Poly([(0, 0), (5, 10), (10, 0)])
	c = Box(((-10, -5), (10, 5)))

	print a.points
	print b.points
	print c.points

