#!/usr/bin/env python
# -*- coding: iso-8859-15 -*-

# simulation.py, Main Simulation Module
# Copyright (C) 2006 por Rafael Treviño Menéndez
# Autor: Rafael Treviño Menéndez <skasi.7@gmail.com>

# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU Library General Public
# License as published by the Free Software Foundation; either
# version 2 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
# Library General Public License for more details.

# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

import ode
import bashcolor
import sys
import os
import libs.drawstuff
import robot
import sequence
import time
import utils
from math import pi, sin, cos, tan

# TODO: Traducir las partes en castellano

#*********************************************#
#* Global Module Variables                   *#
#*********************************************#
# World
world = None

# Collision space
space = None

# Contact group
contactgroup = None

# Variable for models and sequences (list of (model, sequence))
models = []

# Floor geom
floor = None

# Time simulated
simulationTime = 0.0

# Total time
totalTime = None

# Render enabled
render = 1

# Running condition
running = True

#****************************************************************#
#************************** NUEVA API ***************************#
#****************************************************************#

def getSimulationTime ():
	global simulationTime
	return simulationTime

def getRobotInitPosition (index = 0):
	global models
	try:
		pos = models [index][0].initCenter
	except:
		pos = None
	return pos

def getRobotLastPosition (index = 0):
	global models
	try:
		pos = models [index][0].lastCenter
	except:
		pos = None
	return pos

#****************************************************************#

#****************************************************************#
#* Funcion de retrollamada de comienzo de la simulacion         *#
#* Se utiliza principalmente para establecer el punto de vista  *#
#* inicial (posicion de la camara en el universo)               *#
#****************************************************************#
def start ():
	global lastTime
	##-- Esta es la posicion de la camara. Se pasan las coordenadas x,y,z
	lastTime = time.time ()
	
	xyz = (-6.0, 0.3, 2.0)
	
	##-- Esta es la orientacion de la camara.
	##-- Las componentes son Pan, Tilt (inclinacion) y Roll
	##-- El valor (0,0,0) indica que la camara esta apuntando en 
	##-- direccion positiva de las X, paralelamente al suelo.
	hpr = (0.0, -19.0, 0.0)
	
	##-- Establecer la posicion de la camara
	libs.drawstuff.dsSetViewpoint (xyz, hpr)

#*********************************************#
#* FUNCION DE RETROLLAMADA.                  *#
#* Se invoca cuando se pulsa una tecla       *#
#*********************************************#
def command (cmd):
	##-- En este ejemplo no se usan las teclas
	print 'Tecla pulsada: %c' % cmd

#****************************************************************#
#* Funcion de retrollamada invocada por dSpaceCollide cuando    *#
#* dos objetos del espacio estan a punto de colisionar          *#
#* El ODE permite que los usuarios avanzados puedan implementar *#
#* su propia rutina de colision. Para los usuarios no expertos  *#
#* que simplemente quieren una colision estandar, esta es la    *#
#* rutina que SIEMPRE deberan usar. No es solo valida para este *#
#* ejemplo, vale para cualquiera. Pero no esta incluida en la   *#
#* libreria del ODE para que se pueda adaptar a otras           *#
#* necesidades.                                                 *#
#****************************************************************#
def nearCallback (data, o1, o2):
	global world, contactgroup
	##-- Obtener los cuerpos asociados
	b1 = o1.getBody ()
	b2 = o2.getBody ()

	##-- Si ya estan conectados por una articulacion, terminar
	if b1 and b2 and ode.areConnected (b1, b2):
		return
 
	##-- Obtener los puntos de contacto, que en realidad son
	##-- articulaciones (devuelve una lista)
	contacts = ode.collide (o1, o2)

	##-- Para cada punto de contacto crear una articulacion
	for c in contacts:
		##-- Crear articulacion y meterla en el grupo contactgroup
		c.setMode (ode.ContactBounce | ode.ContactSoftCFM)
		c.setMu (0.01)
		c.setMu2 (0)
		c.setBounce (0.1)
		c.setBounceVel (0.1)
		c.setSoftCFM (0.01)
		j = ode.ContactJoint (world, contactgroup, c)
		##-- Establecer la articulacion entre los dos cuerpos
		j.attach (b1, b2)

def stop ():
	global running
	running = False

#***********************************************************************#
#* Realizar un paso de simulacion. Primero se comprueba colisiones. Si *#
#* hay alguna, se crean automaticamente (por medio de la funcion       *#
#* nearCallback, puntos de contactos que en realidad son articulaciones*#
#* Despues se realiza un paso de la simulacion. Los objetos rotaran en *#
#* contacto rotaran sobre los nuevos puntos de contacto creados.       *#
#* Finalmente se eliminan. Si esta en pausa solo de dibuja el universo.*#
#***********************************************************************#
def step (pause):
	global world, space, contactgroup, simulationTime
	if not pause:
		# Servo loop
		for model, seq in models:
			model.setServos (seq)

		# Collision detection
		spaceCollide ()

		# Make a simulation step
		world.step (.05)
		simulationTime += .05

		# Check if the time is over
		if totalTime and simulationTime >= totalTime:
			libs.drawstuff.dsStop ()

		# Remove all the contact points created
		contactgroup.empty ()

	# Draw it!
	for model, seq in models:
		model.drawRobot ()
	
	time.sleep (0.04)

# Done
def initSim (modelfiles, seqfiles, dModule = 0, wire = 0, serial = 0):
	global world, space, contactgroup, models, floor, simulationTime

	# Create a world
	world = ode.World ()

	# Set Gravity
	world.setGravity ( (0, 0, -9.81) )
	
	# Set CFM parameter
	world.setCFM (1e-5)

	# Auto-disabled mode on
	world.setAutoDisableFlag (1)

	# Other parameters of the world simulated
	world.setContactMaxCorrectingVel (0.1)
	world.setContactSurfaceLayer (0.001)

	# Create a collision space
	space = ode.HashSpace ()

	# Create a contactgroup for collisions
	contactgroup = ode.JointGroup ()

	# Floor plane geometry
	floor = ode.GeomPlane (space, (0, 0, 1), 0)

	# Initialize models
	models = []

	# Initialize time
	simulationTime = 0.0

	# Check if models and sequences are compatible
	if len (modelfiles) != len (seqfiles):
		bashcolor.msg ([('VERDE', 'mrs:'),
			('rojo', 'Error:'),
			('cyan', "Not same number of models than sequences.")])
		sys.exit (-1)

	# Loop over the list
	for i, modeli in enumerate (modelfiles):

		# Create a new model in this world and collision space
		model = robot.robot (modeli, world, space, (0.0, 2.0 * i, 0.5), dModule, wire, serial)

		# Check if it's a tree
		if not utils.isATree (model):
			bashcolor.msg ([('VERDE', 'mrs:'),
				('amarillo', 'Warning:'),
				('cyan', "Robot model %d'th isn't a tree. Ignoring..." % (i + 1))])
			
			# Ignore it
			continue

		# Create a new sequence (if exists)
		if seqfiles [i]:
			seq = sequence.sequence (seqfiles [i])
		else:
			seq = None

		if seq and len (seq.gait [0]) != len (model.structure):
			bashcolor.msg ([('VERDE', 'mrs:'),
					('amarillo', 'Warning:'),
					('cyan', "Robot model and movement sequence %d'th are incompatibles. Ignoring..." % (i + 1))])

			# Ignore it
			continue

		# All's OK
		# Append the new created pair model, sequence
		models.append ( (model, seq) )

	# Create all the models in this world and space
	for model, seq in models:

		# Create the model in the ODE world
		model.createRobot ()

##-- Function for make contacts between floor and modules
def spaceCollide ():
	global floor, models, world, contactgroup
	for model, seq in models:
		for geom in model.geoms:
			##-- nearCallback (None, floor, geom)
			##-- Obtener los cuerpos asociados
			b1 = floor.getBody ()
			b2 = geom.getBody ()
 
			##-- Obtener los puntos de contacto, que en realidad son
			##-- articulaciones (devuelve una lista)
			contacts = ode.collide (floor, geom)

			##-- Para cada punto de contacto crear una articulacion
			for c in contacts:
				##-- Crear articulacion y meterla en el grupo contactgroup
				c.setMode (ode.ContactBounce | ode.ContactSoftCFM)
				c.setMu (0.1)
				c.setBounce (0.1)
				c.setBounceVel (0.1)
				c.setSoftCFM (0.01)
				j = ode.ContactJoint (world, contactgroup, c)
				##-- Establecer la articulacion entre los dos cuerpos
				j.attach (b1, b2)

# Done
def simulationLoop (args, endTime = None, renderEnabled = 1):
	"""Main simulation loop."""
	global world, space, totalTime, render, simulationTime, contactgroup, running

	# Set the total amount of time
	if endTime:
		totalTime = simulationTime + endTime
	else:
		totalTime = None

	# Set render or not
	render = renderEnabled

	# Initialize the functions vector with default values
	if render:
		texturesPath = os.path.abspath (os.path.dirname (__file__)) + '/textures'
		fn = (2, start, step, command, None, None, texturesPath)

		# Call the real magic function
		libs.drawstuff.dsSimulationLoop (len (args), args, 500, 400, fn, render)

	else: # This is for speed when not in render mode

		# Set running -> True
		running = True

		while (running):

			# Collision detection
			spaceCollide ()

			# Make a simulation step
			world.step (0.05)
			simulationTime += 0.05

			# Check if the time is over
			if totalTime and simulationTime >= totalTime:
				stop ()

			# Servo loop
			for model, seq in models:
				model.setServos (seq)

			# Remove all the contact points created
			contactgroup.empty ()

			for model, seq in models:
				dobledim = model.dim * 2

				# Total position
				tPos = zip (*[g.getPosition () for b, c, g in model.bodies])
				tPos = [sum (l) / dobledim for l in tPos]

				if model.initCenter:
					model.lastCenter = tPos
				else:
					model.initCenter = tPos

if __name__ == '__main__':
	print 'Autotesting...'
	# Initialize the simulation (list of models, list of sequences)
	initSim (['../quad.rbt'],
			    ['../quad.mvm'],
			    0, 0, None)

	# Begin the simulation!
	for i in xrange (20):
		simulationLoop (sys.argv, 5.0, 0)

		init = getRobotInitPosition (0)
		last = getRobotLastPosition (0)
		print getSimulationTime (), utils.distance (init, last)