
# 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/>.

"""Some sorta pong/tennis/juggling thing...
"""

import Model as m
from Interface01 import Interface01
import math
from math import pi

fcolor1 = (1., 0.5, 0.)
fcolor2 = (0., 0.5, 1.)

class Game(object):

	def __init__(self, Controller):

		worldBounds = ((-10., 0.),(10., 10.))

		ball  = m.Ball((9., 5.), 0.1, restitution=0.90, color=(100, 255, 150), velocity=(-5., 0.), layers=0x1, name="Ball", contacts=True)
		mound = m.FixedPoly([(-7., 0.), (0., 0.5), (7., 0.)], color=(0, 200, 0), name="Mound")

		bodies = [ball, mound]
		joints = []

		p1 = Player(self, (-8., 0.), fcolor1)
		p2 = Player(self, ( 8., 0.), fcolor2)

		bodies += p1.bodies + p2.bodies
		joints += p1.joints + p2.joints

		self.world = m.World(worldBounds, bodies, joints, bgcolor=(0, 0, 0))
		self.world.watch(self.update)

		self.players = [p1, p2]
		self.hands   = [p1.hand, p2.hand]
		self.ball    = ball

		self.score     = 0.
		self.balltime0 = 0.		# Time of last ball-related score adjustment.  Used because we score based on in-flight time of ball.

		self.interfaces = [p1.interface, p2.interface]

		self.controller = Controller(self.interfaces)

		self.scoretime = 0.
		self.lastscore = self.score

	def update(self, model):

		time = model.time

		#
		# Game logic:
		#
		ballside  = math.sin(time*pi/10.)	# Range -1 to 1. ; each side for 10 seconds.
		ballside2 = (ballside + 1.)/2.		# Range  0 to 1.

		bcolor = m.cmix(fcolor2, fcolor1, ballside2)

		self.ball.setColor(m.icolor(bcolor))

		for p in self.players:
			iface = p.interface
			iface.clearEvents()
			iface.ballpos = self.ball.position
			iface.ballvel = self.ball.velocity
			iface.ballrot = self.ball.rotating
			iface.ballcol = ballside

		#
		# Generate a "sound" event when ball hits anything but hands:
		#
		if self.ball.contacts:
			for c in self.ball.contacts:
				if c.body not in self.hands:
					self.sound(c.worldpos, c.impulse, self.balltime0 - time)	# Always negative reward
					self.balltime0 = time

		#
		# Let the players do their thing (also handle hand-ball contact here):
		#
		for p in self.players:
			p.update(time, ballside)

		self.controller.update(time)

		for p in self.players:
			p.updateControls()

		if time > self.scoretime + 60.:	# Print score change every minute
			print "Score change over last minute: %s"%(self.score - self.lastscore,)
			self.lastscore = self.score
			self.scoretime = time

	def sound(self, pos, mag, reward=0.):
		"""This is called whenever we hear a sound (for now, collisions).
		Pos is the position in world coordinates.
		Mag is the volume of the sound.
		reward is a "goodness" measure of the sound, with 0 being neutral,
		  >0 being pleasurable, and <0 being painful.
		"""
		for p in self.players:
			p.interface.sound.append((pos, mag, reward))
		self.score += reward
		if reward > 0.:
			print "Sound(%s,%s,%s)"%(pos, mag, reward)


class Player(object):

	heft = 0.05	# Half the thickness of the rods.

	def __init__(self, game, pos, fcolor):

		self.game      = game
		#self.interface = Interface01(pos, torquesmax = [15., 5., 2.], cost=.1)
		self.interface = Interface01(pos, torquesmax = [4., 2., 1.], cost=.1)

		x, y = pos

		dir = x >= 0. and 1. or -1.

		base = m.FixedBox(
			( (x-.25, y), (x+.25, y+0.25) ),
			color = m.icolor(fcolor, 0.7), name="Base")

		arm1 = m.Bar((x    , y+0.25), (x,     y+1.25), self.heft, color = m.icolor(fcolor,0.9), layers=0x2, density=0.2, name="Arm1")
		arm2 = m.Bar((x    , y+1.25), (x-dir, y+1.25), self.heft, color = m.icolor(fcolor,0.9), layers=0x2, density=0.2, name="Arm2")
		hand = m.Bar((x-dir, y+1.25), (x-dir, y+1.75), self.heft, color = m.icolor(fcolor,0.5), name="Hand", contacts=True)

		a1 =  3.*pi/8.
		a2 = 11.*pi/8.

		if dir > 0.:
			a1, a2 = a2, a1

		fric = 0.5
		self.joints = [
			m.Pivot(base, arm1, (x    , y+0.25), friction=fric),
			m.Pivot(arm1, arm2, (x    , y+1.25), friction=fric, minAngle=-a1, maxAngle=a2),
			m.Pivot(arm2, hand, (x-dir, y+1.25), friction=fric, minAngle=-a2, maxAngle=a1)
			]

		# Middle of the angle range for each pivot:
		self.a0 = [
			0.,
			(a2-a1)/2.,
			(a1-a2)/2.
			]

		self.bodies = [base, arm1, arm2, hand]

		self.hand = hand
		self.side = dir	# +1. = right, -1. = left

	def touch(self, pos, mag, reward):
		"""This is called whenever we touch something with our hand.
		Pos is the position in local coordinates (to the hand) of the sensation.
		Mag is a scalar magnitude of the impact.
		reward is a "goodness" measure of the sensation, with 0 being neutral,
		  >0 being pleasurable, and <0 being painful.
		"""
		self.interface.touch.append((pos, mag, reward))
		self.game.score += reward
		if reward > 0.:
			print "Touch/%s(%s,%s,%s)"%(self.side, pos, mag, reward)

	def update(self, time, ballside):
		"""Time is current sim time.
		Ballside is ball's _preferred_ side, from -1 for left to 1 for right.
		"""
		iface = self.interface

		for i in range(3):
			iface.angles[i] = self.joints[i].angle - self.a0[i]
			iface.angvel[i] = self.joints[i].speed

		iface.handpos = self.hand.position
		iface.handpos = (iface.handpos[0] - iface.position[0], iface.handpos[1] - iface.position[1])

		for c in self.hand.contacts:
			if c.body is self.game.ball:
				reward = ballside * self.side * (time - self.game.balltime0)
				self.game.balltime0 = time
				reward *= 0.5	# Split scoring impact between sound and touch.
				self.game.sound(c.worldpos, c.impulse, reward)
			else:
				#
				# Pain from hitting anything other than the ball:
				#
				reward = -c.impulse
			self.touch(self.hand.toLocal(c.worldpos), c.impulse, reward)

	def updateControls(self):

		iface = self.interface
		game  = self.game
		ts    = game.world.timestep
		cost  = iface.cost

		for i in xrange(3):

			t    = iface.torques[i]
			maxt = iface.torquesmax[i]

			if t > maxt:
				t = maxt
			elif t < -maxt:
				t = -maxt

			self.joints[i].torque = t

			t /= maxt
			game.score -= (t*t) * cost * ts

