﻿import csv
import copy
import numpy as np
import matplotlib
matplotlib.use('Agg')
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import abc
from utils import num2str, Plot
import matplotlib.colors as colors
import matplotlib.cm as cmx
import matplotlib.colorbar as clb
import itertools
import utils
from utils import *
"""robot_Simulation contains the required classes for defining and manipulating the essential robot parameters and states"""
# EXTEND non linear springs morphology (refactor methods to morphology)
# EXTEND non sine control of morphology
# EXTEND create empty robot??

class HardEnvironment(object):
	""" A class for the compact bundling of environment parameters
	the parameters of the environment cannot be optimised and represent physical quantities 
	The difference between hard and soft Environment is that Hard environment incorpates ground effects in the update step,
	 thus resulting in a truly hard ground, while soft environment uses the approach of: Hermans, M. 2014. Automated design of complex dynamic systems

	--- data attributes ---
		- gravity : float
			value of the gravity constant
		- groundFriction : float
			ground friction constant
		- groundContact : float
			contact of the ground variable
		- airFriction : float
			air friction constant
		- threeD : boolean
			is the enivironment for 3D simultations?
	"""
	param = ["gravity", "airFriction", "threeD", "ground"];
	def __init__(self,gravity = 9.81, airFriction = 0.1, threeD = False):
		"""--- Initialise a Robot Environment ---       

			-- parameters --
				- airFriction : float
					air friction constant
				- gravity : float
					gravity constant
				- groundContact : float
					contact of the ground variable
				- groundFriction: float
					ground friction constant

		"""
		self.gravity = gravity
		self.airFriction = airFriction
		self.threeD = threeD
		self.ground = True

	def environmentForce(self, morph, state):
		""" calculate forces generated by the environment returns Vector Fenv asks a morphology and a state"""
		fair = -self.airFriction * state.speed
		fgravy = - self.gravity * morph.mass # ground friction force
		fenv =  fair
		fenv.y = fair.y  + fgravy 
		return fenv

class SoftEnvironment(object):
	""" A class for the compact bundling of environment parameters
	the parameters of the environment cannot be optimised and represent physical quantities

	--- data attributes ---
		- gravity : float
			value of the gravity constant
		- groundFriction : float
			ground friction constant
		- groundContact : float
			contact of the ground variable
		- airFriction : float
			air friction constant
		- threeD : boolean
			is the enivironment for 3D simultations
	"""
	param = ["gravity", "groundFriction", "airFriction", "threeD", "ground"];
	def __init__(self,gravity = 9.81, groundFriction = 100, groundContact = 10, airFriction = 0.1, threeD = False):
		"""--- Initialise a Robot Environment ---       

			-- parameters --
				- airFriction : float
					air friction constant
				- gravity : float
					gravity constant
				- groundContact : float
					contact of the ground variable
				- groundFriction: float
					ground friction constant

		"""
		self.gravity = gravity
		self.groundFriction = groundFriction
		self.groundContact = groundContact
		self.airFriction = airFriction
		self.threeD = threeD
		self.ground = False;

	def environmentForce(self, morph, state):
		""" calculate forces generated by the environment returns Vector Fenv asks a morphology and a state"""
		fair = -self.airFriction * state.speed

		fgravy = - self.gravity * morph.mass # ground friction force

		fgroundy = self.groundContact * np.exp(-self.groundContact* state.pos.y) # ground normal force  # * (state.ypos < 0)
		ffric = self.groundFriction*state.pos.y*state.pos.y* (state.pos.y < 0)
		fgroundx = -ffric * state.speed.x 
		if self.threeD: 
			fgroundz = -ffric * state.speed.z
			fenv = fair + SpaceList(fgroundx, fgroundy + fgravy, fgroundz)
		else:
			fenv =  fair + SpaceList(fgroundx, fgroundy + fgravy)
		return fenv

class Morphology(object):
	__metaclass__  = abc.ABCMeta
	""" An abstract class for the compact bundling of morphological parameters
	the parameters of the morphology can be optimised
	-- data attributes --
			- noNodes : integer
				number of nodes of one array
			- mass : array
				mass of the nodes
			- nodeIds : array
				id number of the nodes
			- connections : 01 matrix
				connection between two nodes?
			- restLength : matrix
				restlength of the spring between nodes
			- initialPos : Vector
				initial coordinates of nodes
			- initHeight : float
				initial height of the robot in construction
			- noNeighbours : int
				number of neighbours to which a spring connection is made
			- environment : Environment
				environement in which the robot will travel
			- threD : boolean
				is the morphology three dimensional
		--- obligatory methods ---
		_connectionForce()
		"""
	param = ['noNodes', 'mass','connections', 'initialPos','environment']

	def __init__(self, initialPos, connections, noNodes, mass, environment):
		"""--- Initialise a morphology of a robot ---
		-- parameters --
			- noNodes : integer
				the number of nodes the robot consists of      
			- mass : float or callable
				the initial mass of the nodes
			- spring : float or callable
				the initial spring constant value
			- damping : float or callable
				the initial damping factor
			- initHeight : float
				initial height of the robot in construction
			- noNeighbours : int
				number of neighbours to which a spring connection is made
			- environment : Environment
				physical environment in which the robot will move
		"""
		# init robot
		self.noNodes = noNodes # the number of nodes the robot consists of
		self.environment = environment
		self.initialPos = initialPos
		self.connections = connections


		# set initial masses
		if utils.isCallable(mass):
			self.mass = mass((self.noNodes,))
		else:
			self.mass = mass * np.ones((self.noNodes,))

		# map connectionNumber to start and end position
		indexMatrix = np.tile(np.arange(noNodes), [noNodes,1])
		self.start = utils.connections2Array(indexMatrix, self.connections)
		self.end = utils.connections2Array(indexMatrix.T, self.connections)

	def connectionParameterMatrix(self, parameter):
		if utils.isCallable(parameter):
			matrix = parameter((self.noNodes, self.noNodes)) * self.connections
			matrix = np.tril(matrix) + np.tril(matrix).T # ensure symmetry
		else:
			matrix = parameter * np.ones((self.noNodes, self.noNodes)) * self.connections
		return matrix

	def getNoConnections(self):
		noConnections = len(utils.connections2Array(self.connections, self.connections))
		return noConnections
	
	@abc.abstractmethod
	def _connectionForce(self, state, modulatedRestLength):
		""" return the force resulting of a connection """

	def plot(self, spring = False): 
		"make a 3D plot of the current morphology"
		# plot settings
		fig = plt.figure(facecolor='white', figsize = (10, 4))

		if self.environment.threeD:
			ax = fig.gca(projection='3d')
			ax.set_zlabel('Z axis')
			ax.view_init(azim=-90, elev=90)
		else: 
			ax = fig.gca()
		ax.set_xlabel('X axis')
		ax.set_ylabel('Y axis')

		radius = 0.3
		def createSpring(xStart, yStart, xEnd, yEnd):
			radius = 0.5
			xLen = xEnd - xStart
			yLen = yEnd - yStart
			len  = np.sqrt(xLen ** 2 + yLen** 2)

			# Rescale
			xStartNew = xStart + radius * xLen / len
			yStartNew = yStart + radius * yLen / len
			xEndNew = xEnd - radius * xLen / len
			yEndNew = yEnd - radius * yLen / len

			xLen = xStartNew - xEndNew
			yLen = yStartNew - yEndNew
			len  = np.sqrt(xLen ** 2 + yLen** 2)

			heightRatio = 2
			twistLen = 0.15
			linewidth = 2
			noTwist = int(len / twistLen)
			xSpace = (xEndNew - xStartNew) * 1.0 / (noTwist)
			ySpace = (yEndNew - yStartNew) * 1.0 / (noTwist)

			X = [xStart, xStartNew]
			Y = [yStart, yStartNew]

			for i in range(noTwist):
				X.append((i + 0.5) * xSpace + xStartNew + (-1)**i * ySpace * heightRatio )
				Y.append((i + 0.5) * ySpace + yStartNew - (-1)**i * xSpace * heightRatio )
	
			X.append(xEndNew)
			X.append(xEnd)
			Y.append(yEndNew)
			Y.append(yEnd)

			plt.plot(X,Y, 'k', linewidth = linewidth)

		# plot lines
		for i, j in itertools.product(range(self.noNodes), range(self.noNodes)):
			if  i < j and self.connections[i,j]:
				mat = np.array([self.initialPos.get(i), self.initialPos.get(j)])
				if np.shape(mat)[1] == 3:
					ax.plot(mat[:,0], mat[:,1], mat[:,2])
				elif spring:
					createSpring(mat[0,0], mat[0,1], mat[1,0], mat[1,1] )
				else: ax.plot(mat[:,0], mat[:,1])
		
		if np.shape(mat)[1] == 3:
			ax.plot(self.initialPos.x, self.initialPos.y, self.initialPos.z, 'k.', markersize = 10)
		else: Plot.circles(self.initialPos.x, self.initialPos.y, radius, 'k')

		if self.environment.threeD:
			xmin , xmax = ax.get_xlim3d()
			margin = 1
			xmin -= margin
			xmax += margin
			x = np.expand_dims( np.arange(xmin, xmax), axis =0)
			z = np.expand_dims( np.arange(xmin, xmax), axis = 1)
			X = np.tile(x,np.shape(z))
			Z = np.tile(z,np.shape(x))
			Y = np.zeros_like(Z)
			Plot.setAxes3Dplot(ax, *ax.get_xlim3d()) # not general
			ax.plot_surface(X, Y, Z, rstride=400, cstride=400, color='#EEEEEE',shade = False)
		
		else:
			Plot.setAxes2Dplot(0.25)
			plt.axis("equal")
		plt.tight_layout(pad = 0)
		plt.axis('off')
		plt.show()
		return fig, ax
	
	def force(self, state, modulatedRestLength):
		""" calculate the force acting on the nodes"""
		# environment force
		fenv = self.environment.environmentForce(self, state)
		fspring = self._connectionForce( state, modulatedRestLength)
		return fenv + fspring

class SpringMorphology(Morphology):
	""" morphology consisting of linear springs, has additional parameters:
			- spring : matrix
				spring constants between nodes
			- damping : matrix
				damping constant between nodes
	"""
	param = ['spring','damping','restLength','initialHeight','noNeighbours'] + Morphology.param

	def __init__(self, noNodes = 20, mass = 1, spring = 1000, damping = 1, initialHeight = 4, noNeighbours = 3, 
		environment = HardEnvironment()):
		
		self.initialHeight = initialHeight  # height of the robot
		self.noNeighbours = noNeighbours  # number of Neighbours
		self.environment = environment
		initialPos = self.generateShape(noNodes, initialHeight)
		connections = self.generateConnections(noNodes)
		super(SpringMorphology, self).__init__(initialPos, connections, noNodes = noNodes,
											 mass = mass, environment = environment)

		# set initial robot spring constants
		self.spring = self.connectionParameterMatrix(spring)

		# set initial damping constants
		self.damping = self.connectionParameterMatrix(damping)
		
		# set rest length
		self.restLength = self.getRestLength(initialPos, connections)

	def loadCSV(self, fileName):
		"""Load the morphology from a config file"""

		with open(fileName, 'r') as csvfile:
			tab = list(csv.reader(csvfile, delimiter=';', quotechar='|'))

			# Get Nodes numbers
			n_node_ind = findIndex(tab, "noNodes")
			n_nodes = int(tab[n_node_ind[0]][n_node_ind[1] + 1])

			# Get initialHeight
			initialHeight_ind = findIndex(tab, "initialHeight")
			initialHeight = int(tab[initialHeight_ind[0]][initialHeight_ind[1] + 1])

			# Get noNeighbours
			noNeighbours_ind = findIndex(tab, "noNeighbours")
			noNeighbours = int(tab[noNeighbours_ind[0]][noNeighbours_ind[1] + 1])


			# Get mass
			mass_ind = findIndex(tab, "mass")
			mass = float(tab[mass_ind[0]][mass_ind[1] + 1])

			# Get spring list
			spring_ind = findIndex(tab, "spring")
			spring_list = []
			for i in range(spring_ind[0] + 1, spring_ind[0] + n_nodes + 1):
				for j in range(spring_ind[1] + 1, spring_ind[1] + n_nodes + 1):
					spring_list.append(float(tab[i][j]))

			# Get damping list
			damping_ind = findIndex(tab, "damping")
			damping_list = []
			for i in range(damping_ind[0] + 1, damping_ind[0] + n_nodes + 1):
				for j in range(damping_ind[1] + 1, damping_ind[1] + n_nodes + 1):
					damping_list.append(float(tab[i][j]))

			# Get restLength list
			restLength_ind = findIndex(tab, "restLength")
			restLength_list = []
			for i in range(restLength_ind[0] + 1, restLength_ind[0] + n_nodes + 1):
				for j in range(restLength_ind[1] + 1, restLength_ind[1] + n_nodes + 1):
					restLength_list.append(float(tab[i][j]))

			# Get connections list
			connections_ind = findIndex(tab, "connections")
			connections_list = []
			for i in range(connections_ind[0] + 1, connections_ind[0] + n_nodes + 1):
				for j in range(connections_ind[1] + 1, connections_ind[1] + n_nodes + 1):
					connections_list.append(float(tab[i][j]))

			# Get initialPos list
			initialPos_ind = findIndex(tab, "initialPos")
			initialPos_list = []
			for i in range(initialPos_ind[0] + 2, initialPos_ind[0] + 4):
				for j in range(initialPos_ind[1] + 2, initialPos_ind[1] + n_nodes + 2):
					initialPos_list.append(float(tab[i][j]))

			# Fill class variables
			#print self.__dict__
			super(SpringMorphology, self).__init__(SpaceList(list2PosMatrix(initialPos_list)), \
				list2SquareMatrix(connections_list), noNodes=n_nodes, mass=mass, \
				environment=self.environment)
			self.initialHeight = initialHeight
			self.noNeighbours = noNeighbours
			self.spring = list2SquareMatrix(spring_list)
			self.damping = list2SquareMatrix(damping_list)
			self.restLength = list2SquareMatrix(restLength_list)

	def _connectionForce(self, state, modulatedRestLength):
		""" calculate the forces acting due to the springs, return x,y spring force"""
		difx, dify = state.pos.getDifference()
		difxy = np.sqrt(difx**2 + dify**2) + np.eye(self.noNodes) #Otherwise divide by zero
		# Normal Linear Spring Definition

		fConnectionx = -np.sum(self.spring * difx * (1 - modulatedRestLength/difxy), axis=1)
		fConnectiony = -np.sum(self.spring * dify * (1 - modulatedRestLength/difxy), axis=1)
		
		#damping forces
		difvx , difvy = state.speed.getDifference()
		fConnectionx += -np.sum(self.damping * difvx, axis=1)
		fConnectiony += -np.sum(self.damping * difvy, axis=1)
		return SpaceList(fConnectionx, fConnectiony)

	def getRestLength(self, initialPos, connections):
		difx, dify = initialPos.getDifference()
		restLength = np.sqrt(difx**2 + dify**2) * connections
		return restLength

	def generateShape(self, noNodes, initialHeight):
		## initialise nodes ##
		# compute initial x and y values (coordinates of the nodes)
		nodeIds = np.arange(noNodes)
		xPos = np.arange(noNodes) - noNodes/2. + .5
		yPos = np.mod(nodeIds+1, 2) * (1-np.abs(2*xPos/noNodes)**2) * self.initialHeight
		initialPos = SpaceList(xPos, yPos)
		return initialPos

	def generateConnections(self, noNodes):
		## initialise connections ##
		# generate the connection matrix
		connections = np.zeros((noNodes, noNodes))
		for i in xrange(self.noNeighbours):
			connections[np.arange(1+i,noNodes), np.arange(0,noNodes-1-i)] = 1  # connect adjacent nodes
		connections += connections.T
		return connections

	def strengthPlot(self):
		# set colorMap
		maxStrength = utils.ceilToRoundNumber(np.max(self.spring))
		jet = plt.get_cmap('Blues') 
		cNorm  = colors.Normalize(vmin = 0, vmax = maxStrength)
		colorMap = cmx.ScalarMappable(norm=cNorm, cmap=jet)

		# calculate relevant properties
		xpos = self.initialPos.x
		ypos = self.initialPos.y

		# plot
		fig = plt.figure(facecolor=(1,1,1), figsize=(10, 6), dpi=150)
		ax = fig.add_axes([0.05, 0.10, 0.85, 0.8])
		for i,j in itertools.product(range(len(xpos)), range(len(ypos))):
			if self.connections[i,j]:
				colorVal = colorMap.to_rgba(self.spring[i,j])
				plt.plot([xpos[i], xpos[j]], [ypos[i], ypos[j]], color = colorVal)
		plt.plot(xpos, ypos, 'ko', markersize = 10)
		plt.fill_between([-12, 12], -20, 0, facecolor='gray', edgecolor='gray')
		
		plt.axis('off')
		Plot.configurePlot(fig, ax, "","", False)
		ax.set_xlim( xmin = -14, xmax = 14)
		ax.set_ylim( ymin = -5, ymax = 15)

		# Add colorbar, make sure to specify tick locations to match desired ticklabels
		cax = fig.add_axes([0.90, 0.10, 0.02, 0.8])
		cbar = clb.ColorbarBase(cax, cmap = jet, ticks=[0,0.25,0.5,0.75,1])
		cbar.ax.set_yticklabels(['0', str(maxStrength / 4),str(maxStrength / 2 ), str(maxStrength/4 * 3), str(maxStrength) ])  # vertically oriented colorbar
		cbar.ax.tick_params(labelsize = 14)
		cbar.ax.set_ylabel('Spring Constant', rotation=270)
		
		Plot.save2eps(fig, 'strengthPlot')

	@staticmethod
	def maxTimeStep(maximumSpringConstant, minimumWeight):

		return np.pi / 5 * np.sqrt( 1.0 * minimumWeight / maximumSpringConstant )

	@staticmethod
	def checkConfig(maximumSpringConstant, minimumWeight, timeStep):

		assert timeStep <= SpringMorphology.maxTimeStep( maximumSpringConstant, minimumWeight )

class SpringMorphology3D(SpringMorphology):
	param = ["square"] + SpringMorphology.param
	def __init__(self, noNodes = 20, mass = 1, spring = 100, damping = 1, initialHeight = 4, noNeighbours = 3, square = False,
				environment = HardEnvironment(threeD = True)):
		assert environment.threeD
		self.square = square
		super(SpringMorphology3D, self).__init__(noNodes = noNodes, mass = mass, spring = spring, damping = damping, initialHeight = initialHeight
												,  noNeighbours = noNeighbours, environment = environment)
	
	def generateShape(self, noNodes, initialHeight):
		## initialise nodes ##
		# compute initial x and y values (coordinates of the nodes)
		self.nodeIds = np.arange(noNodes)
		
		xPos = np.arange(noNodes) - noNodes/2. + .5
		sizeModulation =  (1-np.abs(2*xPos/noNodes)**2) * self.initialHeight;
		if self.square:
			#arrange in square
			zPos = np.mod(self.nodeIds+ 1, 4)/2 * sizeModulation
			yPos = np.mod(self.nodeIds+1, 2) * sizeModulation
		else: 
			# arrange in triangle
			zlist = np.array([-0.5, 0, 0.5])
			ylist = np.array([0, 1, 0])
			zPos = zlist[np.mod(self.nodeIds, 3)] * sizeModulation
			yPos = ylist[np.mod(self.nodeIds, 3)] * sizeModulation
		initialPos = SpaceList(xPos, yPos,zPos)
		return initialPos

	def generateConnections(self, noNodes):
		## initialise connections ##
		# generate the connection matrix
		connections = np.zeros((noNodes, noNodes))
		self.stressed = np.zeros((noNodes, noNodes))
		
		longRange = 5;
		for i in xrange(self.noNeighbours-longRange): 
			startNo = np.mod(np.arange(1 + i,noNodes + i + 1), noNodes)
			endNo = np.mod(np.arange(0, noNodes), noNodes)
			connections[startNo, endNo] = 1  # connect adjacent nodes of outer circle
		for i in xrange(longRange): 
			startNo = np.mod(np.arange(noNodes) + (noNodes - longRange) / 2 + i, noNodes)
			endNo = np.arange(noNodes)
			connections[startNo, endNo] = 1  # connect adjacent nodes of outer circle
		self.stressed[startNo, endNo] = 1
		return connections

	def getRestLength(self, initialPos, connections):
		difx, dify, difz = initialPos.getDifference()
		restLength = np.sqrt(difx**2 + dify**2+ difz**2) * connections
		return restLength

	def _connectionForce(self, state, modulatedRestLength):
		""" calculate the forces acting due to the springs, return x,y,z spring force"""
		difx, dify, difz = state.pos.getDifference()
		difxyz = np.sqrt(difx**2 + dify**2+difz**2) + np.eye(self.noNodes) #Otherwise divide by zero
		# Normal Linear Spring Definition
		fConnectionx = -np.sum(self.spring * difx * (1 - modulatedRestLength/difxyz), axis=1)
		fConnectiony = -np.sum(self.spring * dify * (1 - modulatedRestLength/difxyz), axis=1)
		fConnectionz = -np.sum(self.spring * difz * (1 - modulatedRestLength/difxyz), axis=1)

		#damping forces
		difvx , difvy, difvz = state.speed.getDifference()
		fConnectionx += -np.sum(self.damping * difvx, axis=1)
		fConnectiony += -np.sum(self.damping * difvy, axis=1)
		fConnectionz += -np.sum(self.damping * difvz, axis=1)
		return SpaceList(fConnectionx, fConnectiony, fConnectionz)

class NonLinearMorphology(SpringMorphology):
	def __init__(self, noNodes = 20, mass = 1, spring = 1000, nonLinRatio = 0.03, damping = 1, initialHeight = 4, noNeighbours = 3, 
				environment = HardEnvironment()):
		self.nonLinRatio = nonLinRatio
		super(NonLinearMorphology, self).__init__(noNodes = noNodes, mass = mass, spring = spring, damping = damping, initialHeight = initialHeight
												,  noNeighbours = noNeighbours, environment = environment)

	def _connectionForce(self, state, modulatedRestLength):
		""" calculate the forces acting due to the springs, return x,y spring force"""
		difx, dify = state.pos.getDifference()
		difxy = np.sqrt(difx**2 + dify**2) + np.eye(self.noNodes) # Otherwise divide by zero

		# Linear Spring Force
		temp = (1 - modulatedRestLength/difxy) * self.spring
		fConnectionx = -np.sum(difx * temp, axis=1)
		fConnectiony = -np.sum(dify * temp, axis=1)

		# non Linear Spring Force
		temp = self.spring * self.nonLinRatio / difxy * (difxy - modulatedRestLength) ** 3 / (modulatedRestLength + 1e-10) ** 2 #IMPROVE epsilon
		fConnectionx += - np.sum(difx * temp , axis = 1)
		fConnectiony += - np.sum(dify * temp , axis = 1)

		#damping forces
		difvx , difvy = state.speed.getDifference()
		fConnectionx += -np.sum(self.damping * difvx, axis=1)
		fConnectiony += -np.sum(self.damping * difvy, axis=1)
		return SpaceList(fConnectionx, fConnectiony)

class BalMorphology(SpringMorphology): 
	" not fully stable"

	def __init__(self, noNodes = 20, mass = 1, spring = 100, damping = 1, initialHeight = 4, noNeighbours = 3,
				environment = HardEnvironment()):

		super(BalMorphology, self).__init__(noNodes = noNodes, mass = mass, spring = spring, damping = damping,
			initialHeight = initialHeight,noNeighbours = noNeighbours, environment = environment)

	def generateShape(self, noNodes, initialHeight):
		## initialise nodes ##
		# compute initial x and y values (coordinates of the nodes)

		noIds = np.arange(noNodes)

		xPos = np.sin(2 * np.pi * noIds/ noNodes) * initialHeight / 2
		yPos = (np.cos(2 * np.pi * noIds/ noNodes) + 1) * initialHeight / 2

		initialPos = SpaceList(xPos, yPos)
		return initialPos

	def getRestLength(self, initialPos, connections):
		difx, dify = initialPos.getDifference()
		restLength = np.sqrt(difx**2 + dify**2) * (connections + 0.0 * self.stressed) # TEMP (with stressed methods)
		return restLength

class RobotState(object):
	""" A class for the compact bundling of the state of a Robot
	the state can be updated by an update function
	-- data attributes --
			- currentTime : float
			- pos : array
				coordinates of the nodes
			- speed : array
				speeds of the nodes in the x-direction
			- shape : array
				shape of all vectors in the robot state
	"""
	param = ['currentTime', 'pos', 'speed', 'shape']

	def __init__(self, currentTime = 0, morph =None, copyState = 0): 
		""" --- initialize the state of the robot based on either a generated robot morphology or an existing state ---
			-- parameters --
			- currentTime : float
			- morph : Morphology
				morphology of the robot
			- control : Control
				Control mechanism for the robot
		"""
		if copyState == 0:
			self.currentTime = currentTime;
			self.pos = SpaceList.copy(morph.initialPos)
			self.shape = self.pos.shape
			self.speed =SpaceList(np.zeros((self.shape),float))
		else:
			self.currentTime = copyState.currentTime
			self.pos = copyState.pos.copy()
			self.shape = self.pos.shape
			self.speed = copyState.speed.copy()

	def getStateParameters(self):
		""" Get the robot state: position, speed, and time
		Return xpos, ypos, xspeed, yspeed and currentTime
		"""
		return self.pos.copy(), self.speed.copy(), self.currentTime

	def copy(self):
		""" create a copy of the current state so this copy will not change"""
		return RobotState(copyState = self)

	def setState(self, pos, speed, t):
		""" Set the robot state: position, speed, and time to the given parameter values"""
		for inputValue in [pos, speed]:
			assert self.shape == inputValue.shape, "wrong shape of input arguments"
		self.pos, self.speed,self.currentTime =  pos.copy(), speed.copy(), t

	def setState2D(self, posx, posy, speedx ,speedy, t):
		""" Set the robot state: position, speed, and time to the given parameter values """
		self.setState(SpaceList(np.array(posx), np.array(posy)), SpaceList( np.array(speedx), np.array(speedy)), t)

	def setState3D(self, posx, posy ,posz , speedx ,speedy, speedz, t):
		""" Set the robot state: position, speed, and time to the given parameter values """
		self.setState(SpaceList(np.array(posx), np.array(posy), np.array(posz)), SpaceList( np.array(speedx), np.array(speedy), np.array(speedz)), t)

	def changeState(self,addt,addpos, addspeed, ground = False):
		""" Add time and add Vectors to the current positions and speed """
		for inputValue in [addpos,addspeed]:
			assert self.shape == np.shape(inputValue), "wrong shape of input arguments"
		self.pos += addpos
		self.speed += addspeed
		self.currentTime += addt
		if ground:
			self.pos.ground(self.speed)

	def addPos(self, addt, addpos, ground = False):
		assert self.shape == np.shape(addpos), "wrong shape of input arguments"
		self.pos += addpos
		self.currentTime+= addt
		if ground:
			self.pos.ground(self.speed)

	def addSpeed(self, addspeed, ground = False):
		assert self.shape == np.shape(addspeed), "wrong shape of input arguments"
		self.speed += addspeed
		if ground:
			self.pos.ground(self.speed)

	def getDistanceDifference(self):
		difx, dify = self.pos.getDifference()
		restLength = np.sqrt(difx**2 + dify**2)
		return restLength

class Control(object): 
	__metaclass__  = abc.ABCMeta
	""" An abstract class for the compact bundling of control parameters
	the parameters of control can be optimised """

	@abc.abstractmethod
	def modulationFactor(self, robotState):
		""" must return array with modulation factors """

class TimeControl(Control):
	__metaclass__  = abc.ABCMeta
	""" abstract class with control signals only based on time """

	@abc.abstractmethod
	def modulationFactorTime(self, time):
		""" must return array with modulation factors """

	def modulationFactor(self, robotState):
		return self.modulationFactorTime(robotState.currentTime)

class StepControl(TimeControl):
	""" control which starts at modulation factor = 1, and at time = stepTiming changes to 1 - stepSize """
	def __init__(self, morph, stepSize, stepTime, fraction = 1):
		self.noConn = morph.getNoConnections()
		self.connections = morph.connections
		self.stepSize = stepSize
		self.stepTime = stepTime

		perm = np.random.permutation(self.noConn)
		self.fractionAct = perm[1: int(self.noConn * fraction)]
	

	def modulationFactorTime(self, time):
		if time > self.stepTime:
			factor = 1 - self.stepSize
		else: factor = 1
		modFactor = np.ones(self.noConn)
		for i in self.fractionAct:
			modFactor[i] = factor
		return utils.array2Connections(modFactor, self.connections)

class ReservoirControl(TimeControl):
	"""  bundling of different control signals to test and use reservoirs """
	def __init__(self, morph):
		self.noConn = morph.getNoConnections()
		self.connections = morph.connections
		self.signal = [lambda x:0 ] * self.noConn
			
	def setSinHarmonics(self, amplitude = 0.2, fundamentalHarmonic = 1):
		for  i in range(self.noConn):
			self.signal[i] = ( lambda x, amplitude = amplitude, i = i, fundamentalHarmonic = fundamentalHarmonic:
								1 + amplitude * np.sin(fundamentalHarmonic * x * i) )

	def setRandSin(self, amplitude = 0.2, minMod = 0 , maxMod = 1):
		for  i in range(self.noConn):
			modSpeed = np.random.uniform(minMod,maxMod)
			phase = np.random.uniform(0, np.pi * 2)
			self.signal[i] = ( lambda x, amplitude = amplitude, phase= phase, modSpeed = modSpeed:
						 1 + amplitude * np.sin(x * modSpeed + phase))

	def setSin(self, amplitude = 0.2, modSpeed = 5):
		for  i in range(self.noConn):
			phase = np.random.uniform(0, np.pi * 2)
			self.signal[i] = lambda x: 1 + amplitude * np.sin(x * modSpeed + phase)

	def setReplRandSin(self, amplitude = 0.2, minMod = 0, maxMod = 1, replicationNumber = 5, fraction = 1):
		i = 0
		maxNum = int(fraction * self.noConn)
		perm = np.random.permutation(self.noConn)
		while  i < self.noConn:
			k = 0
			modSpeed = np.random.uniform(minMod,maxMod)
			phase = np.random.uniform(0, np.pi * 2)
			while k < replicationNumber and i < self.noConn:
				if i <= maxNum:
					self.signal[perm[i]] = ( lambda x, amplitude = amplitude, phase= phase, modSpeed = modSpeed:
							 1 + amplitude * np.sin(x * modSpeed + phase))
				else: 
					self.signal[perm[i]] =  lambda x: 1
				i += 1
				k += 1

	def modulationFactorTime(self, time):
		modFactor = np.zeros((self.noConn,1))
		for i in range(self.noConn):
			modFactor[i] = self.signal[i](time)
		return utils.array2Connections(modFactor, self.connections)

class SineControl(TimeControl):
	""" Specific instance of Control Class with sines, works on spring morphologies

	-- data attributes  --  
			- amplitude : matrix
				amplitude of the sine wave
			- phase : matrix
				phase of the sine wave
			- omega : matrix
				speed of modulation
	""" 
	uniform2pi = lambda shape: np.random.uniform(0, 2*np.pi, shape)
	param = ["amplitude", "phase", "omega"]

	def __init__(self, morph, amplitude=0.2, phase=uniform2pi, omega=2*np.pi):
		""" --- Initialise a Control Structure for a Robot based on sines ---

		-- parameters --
			- amplitude : float or callable
				amplitude of the sine wave
			- phase : float or callable
				phase of the sine wave
			- omega : float
				speed of modulation
			- morphology : Morphology
				robot on which the control will be implemented (only connections matrix is important)
		""" 

		## initialise modulation paramters ##
		self.amplitude = morph.connectionParameterMatrix(amplitude)
		self.phase = morph.connectionParameterMatrix(phase)
		self.omega = morph.connectionParameterMatrix(omega)

		self.base_omega = copy.copy(np.matrix([omega]))

	def loadCSV(self, fileName):
		"""Load the controller from a config file"""

		with open(fileName, 'r') as csvfile:
			tab = list(csv.reader(csvfile, delimiter=';', quotechar='|'))

			# Get Nodes numbers
			n_node_ind = findIndex(tab, "noNodes")
			n_nodes = int(tab[n_node_ind[0]][n_node_ind[1] + 1])

			# Get amplitude list
			amplitude_ind = findIndex(tab, "amplitude:")
			amplitude_list = []
			for i in range(amplitude_ind[0] + 1, amplitude_ind[0] + n_nodes + 1):
				for j in range(amplitude_ind[1] + 1, amplitude_ind[1] + n_nodes + 1):
					amplitude_list.append(float(tab[i][j]))

			# Get phase list
			phase_ind = findIndex(tab, "phase:")
			phase_list = []
			for i in range(phase_ind[0] + 1, phase_ind[0] + n_nodes + 1):
				for j in range(phase_ind[1] + 1, phase_ind[1] + n_nodes + 1):
					phase_list.append(float(tab[i][j]))

			# Get omega list
			omega_ind = findIndex(tab, "omega:")
			omega_list = []
			for i in range(omega_ind[0] + 1, omega_ind[0] + n_nodes + 1):
				for j in range(omega_ind[1] + 1, omega_ind[1] + n_nodes + 1):
					omega_list.append(float(tab[i][j]))

			# Fill class variables
			self.amplitude = list2SquareMatrix(amplitude_list)
			self.phase = list2SquareMatrix(phase_list)
			self.omega = list2SquareMatrix(omega_list)

	def getParams(self):
		""" Get the robot parameters: amplitude, phase and modulation speed"""
		return self.amplitude, self.phase, self.omega

	def setParams(self, a, p, omega):
		""" Set the robot parameters: amplitude, phase and modulation speed"""

		self.amplitude, self.phase, self.omega = a, p, omega

	def modulationFactorTime(self, time):
		""" Return rest length modulation factor """
		return 1 + self.amplitude * np.sin(self.omega * time + self.phase)

	def getPower(self, springConstants, springLengths):
		""" Return iteration power """

		return np.sum( (springLengths * self.amplitude) ** 2 * self.omega * springConstants) / 2

class NoisySineControl(SineControl):
	""" Add some noise of the controller signals to ensure robustness towards this in optimization"""

	uniform2pi = lambda shape: np.random.uniform(0, 2*np.pi, shape)
	param = ["amplitude", "phase", "omega"]

	def __init__(self, morph, amplitude=0.2, phase=uniform2pi, omega=2*np.pi, noise=0.2):
		""" Pass the init to parent and add a noise parameter """

		super(NoisySineControl, self).__init__(morph, amplitude=amplitude, \
			phase=phase, omega=omega)

		self.noise = noise
		self.ampli_max = np.max(self.amplitude)
		self.sigma = self.noise * self.ampli_max

	def modulationFactorTime(self, time):
		""" Return rest length modulation factor """

		s = np.random.normal(0, self.sigma, self.amplitude.shape)
		s[self.amplitude==0] = 0
		return 1 + (s + self.amplitude) * np.sin(self.omega * time + self.phase)

class ClosedLoopSineControl(SineControl):
	""" This class extend can be used for controlling the robot in closed-loop:
	 - If the CL parameter is se to False, it acts in open loop exactly like its parent class
	SineControl.
	 - If the CL parameters is change to True, the modulation factor is now given directly
	through an argument by the simulation class.
	Note that is should only been used with a TrainingSimulation simulator """

	uniform2pi = lambda shape: np.random.uniform(0, 2*np.pi, shape)
	firstStep = True

	def __init__(self, morph, amplitude=0.2, phase=uniform2pi, omega=2*np.pi):
		""" Init class """

		super(ClosedLoopSineControl, self).__init__(morph, amplitude=amplitude, phase=phase, omega=omega)

		self.CL = False
		self.stepInput = np.array([])
		self.closingLength = 0
		self.closedLoopLength = 0
		self.closingStep = 0
		self.morph = morph

	def closeLoop(self, closedLength, beta=0.1):
		""" Call this function to close the loop """

		self.CL = True
		self.beta = float(beta)
		self.closedLength = closedLength

	def setStepInput(self, predSig):
		""" Set the controller output for the next time step from the simulation outputs """

		self.stepInput = array2ModFactor(predSig, self.morph.connections)

	def modulationFactorTime(self, time):
		""" Redefine mod factor method in closed-loop"""

		# If open-loop mode
		if self.CL == False:
			modFactor = super(ClosedLoopSineControl, self).modulationFactorTime(time)
			return modFactor

		# If closed-loop mode
		else:
			ol = super(ClosedLoopSineControl, self).modulationFactorTime(time)

			#print str(time) + " Getting ol control signal"
			cl = self.stepInput

			if self.closingStep == 0:
				print(" -- Starting to close the loop at time " + str(time) + "s --")

			if self.closingStep < self.closedLength:
				factor = self.beta * self.closingStep / self.closedLength
				if factor > 1:
					factor = 1
				modFactor = factor * cl + (1 - factor) * ol
			else:
				modFactor = cl

			self.closingStep += 1
			#print str(time) + " Mixing and applying cl+ol control signal " 
			return modFactor

class GenerativeControl(Control):
	#URGENT check for syntax and logic errors (not yet used)

	def __init__(self, map, buffer, morph, delayTime, delayStep):
		self.map = map
		self.buffer = buffer
		self.noConnections = morph.getNoConnections()
		self.conStart = morph.start
		self.conEnd = morph.end
		self.delayTime = delayTime
		self.delayStep = delayStep
		assert len(buffer[0]) == delayStep * delayStep, ("A buffer of the wrong size is given :"+
													 str(len(buffer)) + "instead of " + str( delayStep * delayTime) ) 

	def modulationFactor(self, robotState):
		springLengths = getSpringLengths(robotState)
		self.addToBuffer(springLengths)
		input = self.getInputfromBuffer()
		output = self.map.value(input) # FIX work also for multidimensional outputs
		return np.tile(output, [self.noConnections])

	def addToBuffer(self, springLengths):
		self.buffer[-1] = springLengths
		for  i in range(springLengths - 1):
			self.buffer[i] = self.buffer[i + 1]

	def getInputfromBuffer(self):
		input = np.zeros( self.noConnections, 0)
		for i in range(delayTime + 1):
			input = np.vstack((input, self.buffer[self.delayStep * (self.delayTime - i) ]))

	def getSpringLengths(self, robotState):
		springLengths = np.zeros(self.noConnections)
		for i in range(self.noConnections):
			node1 = self.conStart[connectionNumber]
			node2 = self.conEnd[connectionNumber]
			springLengths[i] = robotState.pos.getDistance(node1, node2)
		return springLengths

class Robot(object):
	""" A class for the compact bundling of the morphology, control system and state of the robot 
	--- data attributes --
			- morph : Morphology
				robot of which the state is described by this class
			- control : Control
				controlling mechanism for the robot
			- state : Robotstate
				current state of the robot

	"""
	param = ["morph", "control"]
	def __init__(self,morph ,control ,state = None):
		""" --- initialize a robot structure ---
		-- parameters --
			- morph : Morphology
				robot of which the state is described by this class
			- control : Control
				controlling mechanism for the robot
			- state : Robotstate
				current state of the robot
				"""
		self.morph = morph
		self.control = control
		if state is not None:
			self.state = state
		else: self.state = RobotState(currentTime = 0, morph = morph)

	def computeForces(self):
		## Forces
		# modulated rest lengths
		modulatedRestLength = self.morph.restLength * self.control.modulationFactor(self.state)
		f = self.morph.force(self.state, modulatedRestLength)
		return f

	def computeAcceleration(self):
		""" compute acceleration, returns Acceleration Vector"""
		f = self.computeForces()
		a = f / self.morph.mass
		return a

	def getVelocity(self):
		""" return a list of x and y velocity """
		return self.state.speed

	@staticmethod
	def getDistanceTraveled(initstate, endstate):
		xMeanStart = np.mean(initstate.pos.x)
		xMeanEnd = np.mean(endstate.pos.x)
		return xMeanEnd - xMeanStart

	def getShape(self):

		return self.state.shape

	def getProperty(self, name):
		""" returns the list of the property with the required name"""

		if (hasattr(self.morph,name)):
			prop = getattr(self.morph, name)
		elif (hasattr(self.control, name)):
			prop = getattr(self.control, name)
		else:
			raise AttributeError('The robot has no such property ('+ name + ')')
			prop = None

		if name.find("base_") == -1:
			return utils.connections2List(prop,self.morph.connections)

		else:
			#print "heeeeeeeeeeerrrrrrrrrrrrrrrrrrrrrrrr", name, prop
			return utils.number2List(prop)

	def setProperty(self, name, liste):
		""" set a property with the required name by list"""
		
		prop = utils.list2Connections(liste, self.morph.connections)

		if(hasattr(self.morph, name)):
			setattr(self.morph, name, prop)
		elif (hasattr(self.control ,name)):
			setattr(self.control, name,prop)
		else:
			raise AttributeError('The robot has no such property ('+ name + ')')
		
	def getParams(self):
		""" Get the robot parameters: amplitude, phase and modulation speed"""
		return self.control.getParams()

	def setParams(self, a, p, omega):
		""" Set the robot parameters: amplitude, phase and modulation speed"""
		return self.control.setParams(a,p,omega)

	def getStateParameters(self):
		""" Get the robot state parameters: position, speed, and time
		Return xpos, ypos, xspeed, yspeed and currentTime
		"""
		return self.state.getStateParameters()

	def getState(self):

		return self.state.copy()

	def changeState(self, timeStep, V, A):
		""" Change the time and positions based on time derivatives """

		posChange = timeStep * V
		speedChange = A * timeStep
		self.state.changeState(timeStep, posChange, speedChange, self.morph.environment.ground)

	def changeStateVerlet(self, timeStep, V, Aold, noise=0, impulsenoise=None):

		if noise is not 0:
			shape = np.shape(V.matrix)
			posNoise = 1 + np.random.standard_normal(shape) * noise
			speedNoise = 1 + np.random.standard_normal(shape) * noise

		posChange = timeStep * V + (timeStep * timeStep ) * Aold / 2
		if noise is not 0:
			posChange *= posNoise
		self.state.addPos(timeStep, posChange, self.morph.environment.ground)
		Anew = self.computeAcceleration()

		if impulsenoise is not None:
			Anew = Anew + utils.SpaceList(impulsenoise)

		# then Force calculation
		# then Speed update
		speedChange = timeStep * (Aold + Anew) /2
		if noise is not 0:
			speedChange *= speedNoise
		self.state.addSpeed(speedChange, self.morph.environment.ground)
		return Anew

	def printState(self):

		state = self.state
		meanPos = np.mean(state.pos.x)
		meanSpeed = np.mean(state.speed.x)
		stdSpeed = np.std(state.speed.x)
		time = state.currentTime
		stateDescription = " at current time:= "+ num2str(time)+ \
			"\nthe average x position is " + num2str(meanPos)+ \
			"\nthe average x speed is " + num2str(meanSpeed)
		return stateDescription

	def getConnections(self):

		return self.morph.connections

	def _getRobotPos2D(self):
		"""get the x and y coordinates of the nodes and the connections matrix"""

		return self.state.pos.x, self.state.pos.y, self.morph.connections

	def robot2liste(self, paramlist):
		""" --- convert the requested robot parameters to a matrix ---
		
		-- parameters --
		- paramlist : list of Strings
			strings with the correct names of the parameters

		-- output --
		- matrix : matrix
			matrix of which the headers are given by paramlist
		"""

		liste = []
		for i in range(len(paramlist)):
			liste.append(self.getProperty(paramlist[i]))
		return liste

	def getNoConnections(self):

		return self.morph.getNoConnections()

	def reset(self):

		self.state = RobotState(0, self.morph)
	
	def list2robot(self, namelist, vallist, reset=True):
		""" --- update the robot to a list of parameters together with a list of names and optionally reset state ---
		-- parameters --
		- namelist : list of Strings
			strings with the correct names of the parameters
		- vallist : list of values
			matrix of which the headers are given by paramlist
		"""

		if reset:
			self.reset()
		for i in range(len(vallist)):
			self.setProperty(namelist[i], vallist[i])
		return self

	def matrix2robot(self,paramlist,matrix, reset = True):
		""" --- update the robot to a matrix of parameters together with a list of parameters and optionally reset state ---
		
		-- parameters --
		- paramlist : list of Strings
			strings with the correct names of the parameters
		- matrix : matrix
			matrix of which the headers are given by paramlist
		"""

		assert np.shape(matrix)[1] == self.getNoConnections(), "wrong number of parameters"
		if reset:
			self.reset()
		for i in range(len(paramlist)):
			self.setProperty(paramlist[i],matrix[i,:])
		return self

	def getDistance(self,connectionNumber):

		node1 = self.morph.start[connectionNumber]
		node2 = self.morph.end[connectionNumber]
		return self.state.pos.getDistance(node1, node2)

	def currentRestLength(self, array = False, delay = 0):

		if delay == 0:
			modFactor = self.control.modulationFactor(self.state)
		else: # requires TimeControl Instance
			modFactor = self.control.modulationFactorTime(self.state.currentTime - delay)
		if array:
			return utils.connections2Array(self.morph.restLength * modFactor, self.morph.connections)
		else:
			return self.morph.restLength * modFactor

	def stressRatio(self):

		return self.state.getDistanceDifference() / self.currentRestLength()

	def getPower(self):

		return self.control.getPower(self.morph.spring, self.morph.restLength)

	def referencePower(self, springConstant, springLength, amplitude, omega):

		return self.getNoConnections() * (amplitude * springLength) ** 2 * springConstant * omega

	def staticPlot(self):

		border = 1.5
		jet = cm = plt.get_cmap('spectral') 
		cNorm  = colors.Normalize(vmin = 0.5, vmax = 1.5)
		colorMap = cmx.ScalarMappable(norm=cNorm, cmap=jet)

		fig, ax  = plt.subplots(figsize=(10.88, 6.88), dpi=100, )
		xpos, ypos, connections = self._getRobotPos2D()
		stressRatio = self.stressRatio()
		plt.cla()
		plt.fill_between([-1e8, 1e8], -20, 0, facecolor='gray', edgecolor='gray')
		maxy = np.max(self.morph.initialPos.y)
		plt.ylim(-border, border + 3 * maxy)
		xplotwidth = max(xpos) + border - (min(xpos) - border)
		for i,j in itertools.product(range(len(xpos)), range(len(ypos))):
			if connections[i,j]:
				colorVal = colorMap.to_rgba(stressRatio[i,j])
				plt.plot([xpos[i], xpos[j]], [ypos[i], ypos[j]], color = colorVal)
		plt.plot(xpos, ypos, 'ko', markersize = 10)
		minx = min(xpos)-border
		plt.xlim(minx, minx + xplotwidth)
		return fig, ax

	def getSumOfSpringLengths(self):
		""" calculate sum of springs lengths (slow computation)"""

		sum = 0
		for  i in range(self.morph.getNoConnections()):
				sum += self.getDistance(i)
		return sum
# endregion

class TargetFunctions:
	@staticmethod
	def squaredLength(robot):
		excess = np.array( robot.currentRestLength(array = False)) - robot.morph.restLength
		sum = np.sum(excess ** 2)
		return sum

	@staticmethod
	def sumLength(robot):
		excess = np.array( robot.currentRestLength(array = False)) - robot.morph.restLength
		sum = np.sum(excess)
		return sum

	@staticmethod
	def memoryHalfSecond(robot):
		excess = np.array( robot.currentRestLength(array = False, delay = 0.5)) - robot.morph.restLength
		sum = np.sum(excess)
		return sum

	@staticmethod
	def oneStepAhead(robot):
		timeStep = 1.0 / 200 # TEMP
		currentTime = robot.state.currentTime
		nextTime = currentTime + timeStep
		predict = robot.control.modulationFactorTime(nextTime)
		predict = utils.connections2Array(predict, robot.morph.connections)
		return predict[0]
