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

# step.py, Movement Generator Library
# 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 sys
from math import *

# Variable for samples per movement
samples = None

# Constant for conversions
DEG_TO_RAD = pi / 180.0

# List of CPG's (Central Pattern Generator)
# -a: amplitude
# -g: gap between servos

# -s: samples
def generateMatrix (cpgs, s):
	global samples

	samples = s
	seq = []
	for f, a in cpgs:
		a = [int (i) for i in a]
		seq.append ([])
		for j in xrange (s):
			b = a + [j]
			seq [-1].append (f (*b))
	return seq

def parseFile (file):
	fd = open (file)
	lines = fd.readlines ()
	fd.close ()
	funcs = {}
	l = []
	for line in lines:
		if '#' not in line: # No comentario		
			if ':' in line: # Function
				parts = line.split (':')
				parts = [part.strip () for part in parts]
				exec ('funcs [parts [0]] = lambda %s: %s' % (parts [1], parts [2]))
			else:
				parts = line.split ()
				if parts: # CPG
					l.append ((funcs [parts [0]], parts [1:]))

	return l

def mrsOutput (seq, fd):
	for module in seq:
		module = ['%d' % value for value in module]
		print >> fd, ' '.join (module)

def cOutput (seq, fd):
	print >> fd, 'static secPP seq = {'
	lines = []
	for module in seq:
		line = '\t{' + ', '.join (['%d' % value for value in module]) + '}'
		lines.append (line)
	fd.write (',\n'.join (lines))
	print >> fd, '\n};'

def octaveOutput (seq, fd):
	global samples

	for i, module in enumerate (seq):
		module = ['%d' % value for value in module]
		fd.write ('f%d = [' % (i + 1))
		fd.write (', '.join (module))
		print >> fd, '];'
	print >> fd, 't = [0 : 1 : %d];' % (samples - 1)
	print >> fd, 'plot (t,', ', t, '.join (['f%d' % (i + 1) for i in range (len (seq))]), ')'
	print >> fd, 'grid on;'
	print >> fd, 'pause;'

if __name__ == '__main__':
	print 'Autotesting...'
	print 'f = lambda x, y, t: x + y + t'
	print '\n* generateMatrix ([(f, (2, 3))], 10)'
	f = lambda x, y, t: x + y + t
	s = generateMatrix ([(f, (2, 3))], 10)
	print s
	print '\n* MRS output'
	mrsOutput (s, sys.stdout)
	print '\n* C output'
	cOutput (s, sys.stdout)
	print '\n* Octave output'
	octaveOutput (s, sys.stdout)