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

# sequence.py, Movement 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 bashcolor
import sys
import types
from math import pi

class sequence:

	def __init__ (self, seq):
		"""Initialize the sequence reading a file of movement"""
		# Actual point of movement
		self.seqPosition = 0
		# Gait matrix
		self.gait = []
		# Number of movements
		self.nMoves = 0
		# seq could be a file or a sequence matrix
		# seq is a file?
		if type (seq) == types.StringType:
			try:
				f = open (seq, 'r')
			except:
				bashcolor.msg ([('VERDE', 'mrs:'),
						('rojo', 'Error:'),
						('cyan', "movement file doesn't exits.")])
				sys.exit (-1)
			gait = zip (*[l.split () for l in f.readlines ()])
			for item in gait:
				self.gait.append ([int (i) for i in item])
			f.close ()
		# seq is a list
		else:
			self.gait = zip (*seq)
		self.nMoves = len (self.gait)
	
	def forward (self):
		"""Forward the sequence being simulated"""
		self.seqPosition = (self.seqPosition + 1) % self.nMoves

	def desiredPos (self):
		"""Gets the actual position"""
		return self.gait [self.seqPosition]

	def writeGait (self, file):
		"""Writes the gait"""
		f = open (file, 'w')
		s = ' '
		for item in self.gait:
			out = [str (i) for i in item]
			f.write (s.join (out))
			f.write ('\n')
if __name__ == '__main__':
	print 'Autotesting...'
	seq = sequence ('../simple.mvm')
	print seq.desiredPos ()
	seq.forward ()
	print seq.desiredPos ()
	seq.forward ()
	print seq.desiredPos ()
	seq.forward ()
	print seq.desiredPos ()
	seq.writeGait ('gait.txt')