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

# utils.py, Miscelaneous Utils 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.

from math import sqrt, sin, cos
from OpenGL.GL import *
import bashcolor

def isATree (robot):
	d = {}
	for i, a in robot.structure:
		if d.has_key (a [0]):
			d [a [0]] = d [a [0]] + 1
		else:
			d [a [0]] = 1
		if d.has_key (a [2]):
			d [a [2]] = d [a [2]] + 1
		else:
			d [a [2]] = 1
	arcs = len (robot.structure)
	nodes = len (d)
	return (nodes == arcs + 1)

def findLeaf (robot):
	d = {}
	for i, a in robot.structure:
		if d.has_key (a [0]):
			d [a [0]] = d [a [0]] + 1
		else:
			d [a [0]] = 1
		if d.has_key (a [2]):
			d [a [2]] = d [a [2]] + 1
		else:
			d [a [2]] = 1
	for a in d:
		if d [a] == 1:
			return a

def findNeighbours (robot, node):
	r = []
	tmp = []
	for i, item in robot.structure:
		if (item [0] == node or item [2] == node):
			r.append ((i, item))
		else:
			tmp.append ((i, item))
	robot.structure = tmp
	return r

def distance (a, b):
	dx, dz = (a [0] - b [0]), (a [2] - b [2])
	return sqrt (dx * dx + dz * dz)

def rotation (angle, axis):
        """Return a rotation matrix."""
        
        sqr_a = axis[0]*axis[0]
        sqr_b = axis[1]*axis[1]
        sqr_c = axis[2]*axis[2]
        len2  = sqr_a+sqr_b+sqr_c

        k2    = cos(angle)
        k1    = (1.0-k2)/len2
        k3    = sin(angle)/sqrt(len2)
        k1ab  = k1*axis[0]*axis[1]
        k1ac  = k1*axis[0]*axis[2]
        k1bc  = k1*axis[1]*axis[2]
        k3a   = k3*axis[0]
        k3b   = k3*axis[1]
        k3c   = k3*axis[2]

        return (k1*sqr_a+k2, k1ab-k3c, k1ac+k3b,
               k1ab+k3c, k1*sqr_b+k2, k1bc-k3a,
               k1ac-k3b, k1bc+k3a, k1*sqr_c+k2)

def loadRobot (f):
	try:
		fd = open (f)
	except:
		bashcolor.msg ([('VERDE', 'mrs:'),
			('rojo', 'Error:'),
			('cyan', "robot file doesn't exits.")])
		sys.exit (-1)
	structure = []
	for line in fd.readlines ():
		structure.append (line.split ())
	for i, item in enumerate (structure):
		item [0] = int (item [0])
		item [-1] = int (item [-1])
		item = (i, item)
	fd.close ()
	return structure