
# Protocol Buffers driver for Interface01 of Invivo AI challenge simulation environment
# Copyright 2009,2010 Gerard Toonstra, 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/>.

import math
import socket
import sys

sys.path.append( "./py" )
from interface01_pb2 import *

class Controller(object):

	"""Test controller for Interface01.
	"""
	def __init__(self, interfaces):
		"""Interfaces is a pair of Interface01 objects."""
		self.bots = interfaces
		self.pipe = socket.socket()

		host = "localhost"
		port = 54325

		print "Connecting to %s:%s"%(host, port)

		self.pipe.connect((host, port))

	def update(self, time):
		state = SimState()
		left = state.left
		right = state.right
		self.setValues( left, self.bots[ 0 ] )
		self.setValues( right, self.bots[ 1 ] )

		state.ballpos.a1 = self.bots[ 0 ].ballpos[ 0 ]
		state.ballpos.a2 = self.bots[ 0 ].ballpos[ 1 ]
		state.ballvel.a1 = self.bots[ 0 ].ballvel[ 0 ]
		state.ballvel.a2 = self.bots[ 0 ].ballvel[ 1 ]
		state.ballcol = self.bots[ 0 ].ballcol

		serialized = state.SerializeToString()
		totalLen = "%04d" % len(serialized)

		self.pipe.send( totalLen + serialized )

		totalLen = int( self.pipe.recv( 4 ) )
		cmdString = self.pipe.recv( totalLen, socket.MSG_WAITALL )

		commands = Commands()
		commands.ParseFromString( cmdString )

		left = commands.left;
		right = commands.right;		

		self.bots[ 0 ].torques[ 0 ] = left.torqueA;
		self.bots[ 0 ].torques[ 1 ] = left.torqueB;
		self.bots[ 0 ].torques[ 2 ] = left.torqueC;

		self.bots[ 1 ].torques[ 0 ] = right.torqueA;
		self.bots[ 1 ].torques[ 1 ] = right.torqueB;
		self.bots[ 1 ].torques[ 2 ] = right.torqueC;

	def setValues( self, player, bot ):
		player.cost = bot.cost
		player.pos.a1 = bot.position[ 0 ]
		player.pos.a2 = bot.position[ 1 ]
		player.angles.a1 = bot.angles[ 0 ]
		player.angles.a2 = bot.angles[ 1 ]
		player.angles.a3 = bot.angles[ 2 ]
		player.angvel.a1 = bot.angvel[ 0 ]
		player.angvel.a2 = bot.angvel[ 1 ]
		player.angvel.a3 = bot.angvel[ 2 ]
		player.handpos.a1 = bot.handpos[ 0 ]
		player.handpos.a2 = bot.handpos[ 1 ]
		player.cost = bot.cost

