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

# simulator.py, Simulator
# 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
sys.path.append ('../generator')
import genyal2, simulation, robot, step, utils, sequence
from math import sin, pi

DEG_TO_RAD = pi / 180

model = None
output = None
samples = 10

func = lambda A, T, Phi, t: int (A * sin ((2 * pi * t) / (T * samples) + DEG_TO_RAD * Phi)) 

def convertToSeq (chromo):
	final = []
	cpgs = []
	for i in xrange (0, len (chromo), 3):
		cpgs.append ((func, (chromo [i] / 18, chromo [i + 1] / 18 + 11, chromo [i + 2])))
	chromo = (chromo [0] / 18, chromo [1] / 18, chromo [3])
	final = step.generateMatrix (cpgs, samples)
	return final

def fitness (chromo):
	simulation.initSim ([model], [convertToSeq (chromo)], 0, 0, None)
	simulation.simulationLoop (sys.argv, 10.0, 0)
	return utils.distance (simulation.getRobotInitPosition (0), simulation.getRobotLastPosition (0))

if __name__ == '__main__':
	if len (sys.argv) < 3:
		print 'optimizer: Use: %s <robot file> <output file>'
		sys.exit (-1)
	model = robot.robot (sys.argv [1], None, None, None, None, None, None)
	model = model.structure
	output = sys.argv [2]
	ga = genyal2.Genyal (M = 5, bases = range (-180, 180), N = len (model) * 3, fitness = fitness, probs = [0.1, 0.5])
	best = 0.0
	gen = 0
	while best < 1.0 and gen < 100:
		ga.nextGen ()
		gen, mean, best = ga.statics ()
		print 'Generation %d' % gen
		print 'Best:', best
		print ga.bestIndividual ()
	final = zip (*convertToSeq (ga.bestIndividual ()))
	seq = sequence.sequence (final)
	seq.writeGait (output)