def dy_Stance(self, t, y, pars, return_force = False):
     """
     This is the ode function that is passed to the solver. Internally, it calles:
         legfunc1 - force of leg 1 (overwrite for new models)
         legfunc2 - force of leg 2 (overwrite for new models)
     
     :args:
         t (float): simulation time
         y (6x float): CoM state
         pars (dict): parameters, will be passed to legfunc1 and legfunc2.
             must also include 'foot1' (3x float), 'foot2' (3x float), 'm' (float)
             and 'g' (3x float) indicating the feet positions, mass and direction of
             gravity, respectively.
         return_force (bool, default: False): return [F_leg1, F_leg2] (6x
             float) instead of dy/dt.
     """
     
     f1 = max(self.legfunc1(t, y, pars), 0) # only push
     l1 = norm(array(y[:3]) - array(pars['foot1']))
     f1_vec = (array(y[:3]) - array(pars['foot1'])) / l1 * f1
     f2 = max(self.legfunc2(t, y, pars), 0) # only push
     l2 = norm(array(y[:3]) - array(pars['foot2']))
     f2_vec = (array(y[:3]) - array(pars['foot2'])) / l2 * f2
     if return_force:
         return hstack([f1_vec, f2_vec])
     return hstack([y[3:], (f1_vec + f2_vec) / pars['m'] + pars['g']])

def solveToF(t1,c1,t2,c2,sign=False):
    if t2 == False:
        return False
    tf1 = t2-t1
    
    c1p = c1.eph(t1)[0]
    c2p = c2.eph(t2)[0]
    
    #c1p[2] = 0.0
    #c2p[2] = 0.0
    
    c1p = norm(c1p)
    c2p = norm(c2p)
    
    tf2 = pi * sqrt((c1p+c2p)**3 / (8*c1.ref.mu))
    #if abs(tf1-tf2) > 3000000:
    #    print "OKAY WIERD SITUATION"
    #    print
    #    print
    #    print
    #    print "TF1",tf1
    #    print "TF2",tf2
    if sign:
        return tf1-tf2
    else:
        return abs(tf1-tf2)

def corr(x,y):
    nx = pl.norm(x)
    ny = pl.norm(y)
    if nx == 0 or ny == 0:
        return 0
    else:
        return pl.dot(x,y) / (nx * ny)

def checkmodelgrad(model,e,RETURNGRADS=False,*args):
    from pylab import norm
    """Check the correctness of passed-in model in terms of cost-/gradient-
       computation, using gradient approximations with perturbances of 
       size e. 
    """
    def updatemodelparams(model, newparams):
        model.params *= 0.0
        model.params += newparams.copy()
    def cost(params,*args):
        paramsold = model.params.copy()
        updatemodelparams(model,params.copy().flatten())
        result = model.cost(*args) 
        updatemodelparams(model,paramsold.copy())
        return result
    def grad(params,*args):
        paramsold = model.params.copy()
        updatemodelparams(model, params.copy().flatten())
        result = model.grad(*args)
        updatemodelparams(model, paramsold.copy())
        return result
    dy = model.grad(*args)
    l = len(model.params)
    dh = zeros(l,dtype=float)
    for j in range(l):
        dx = zeros(l,dtype=float)
        dx[j] = e
        y2 = cost(model.params+dx,*args)
        y1 = cost(model.params-dx,*args)
        dh[j] = (y2 - y1)/(2*e)
    print "analytic: \n", dy
    print "approximation: \n", dh
    if RETURNGRADS: return dy,dh
    else: return norm(dh-dy)/norm(dh+dy)

def checkgrad(f,g,x,e,RETURNGRADS=False,*args):
    from pylab import norm
    """Check correctness of gradient function g at x by comparing to numerical
       approximation using perturbances of size e. Simple adaptation of 
       Carl Rasmussen's matlab-function checkgrad."""
    # print f
    # print g
    # print x
    # print e
    # print RETURNGRADS
    # print args
    
    dy = g(x,*args)
    if isscalar(x):
        dh = zeros(1,dtype=float)
        l = 1
    else:
        print "x in checkgrad:"
        print x 
        l = len(x)
        dh = zeros(l,dtype=float)
    for j in range(l):
        dx = zeros(l,dtype=float)
        dx[j] = e
        y2 = f(x+dx,*args)
        y1 = f(x-dx,*args)
        #print dx,y2,y1
        dh[j] = (y2 - y1)/(2*e)
        #print dh[j]
    print "analytic (using your gradient function): \n", dy
    print "approximation (using the objective function): \n", dh
    if RETURNGRADS: return dy,dh
    else: return norm(dh-dy)/norm(dh+dy)

def getAngle(t1,c1,t2,c2):
    ''' 
    Get angle between two celestials at t1 and t2 
    
    Verify if ignoring the k-cordinate makes any sense
    
    timeit 240 microseconds
    '''
    if type(t2) == numpy.ndarray:
            t2 = t2[0]
    elif isnan(t2):
        print "ERROR, t2 is nan!"
        return t2
    
    p1 = c1.eph(t1)[0]
    p1[2] = 0.0
    p1l = norm(p1)
    p1 /= p1l
    
    p2 = c2.eph(t2)[0]
    p2[2] = 0.0
    p2l = norm(p2)
    p2 /= p2l
    
    #if p1l > p2l:
    return p1.dot(p2)
    #else:
    #    return p1.dot(p2)
    
    '''

def getMohoEvePA(t):
    moho = tk.Moho.eph(t)[0][:2]
    eve = tk.Eve.eph(t)[0][:2]
    
    moho = moho / norm(moho)
    eve = eve / norm(eve)
    
    return degrees(arccos(moho.dot(eve)))

def getMohoKerbinPA(t):
    moho = tk.Moho.eph(t)[0][:2]
    kerbin = tk.Kerbin.eph(t)[0][:2]
    
    moho = moho / norm(moho)
    kerbin = kerbin / norm(kerbin)
    
    return degrees(arccos(moho.dot(kerbin)))

    def treinar(self, eta, max_iteracoes, treinamento, teste, dimension):
        """
		Exibe a interface contendo o conjunto de dados,
		a reta separadora iniciada e as iterações do algoritmo
		até a convergência ou o limite de iterações seja
		atingido.
		"""
        self.showing_train_data = True
        self.trainset = treinamento  # train set generation
        self.perceptron = Perceptron(eta, max_iteracoes, dimension)  # perceptron instance
        self.perceptron.train(self.trainset)  # training
        self.testset = teste  # test set generation
        self.x = 0
        self.y = 0

        plt.ion()
        self.fig = plt.figure()
        self.ax = self.fig.add_subplot(111)

        # plot of the separation line.
        # The separation line is orthogonal to w

        self.fig.canvas.draw()
        self.ax.set_title("starting perceptron. traning data:")

        for y in self.trainset:
            if y[dimension] == 1:
                self.ax.plot(y[0], y[1], "oc")
            else:
                self.ax.plot(y[0], y[1], "om")

        self.fig.canvas.draw()
        sleep(2)
        self.ax.set_title("initial separation line:")

        w0 = self.perceptron.getHistory()[0]
        n = norm(w0)
        ww = w0 / n
        ww1 = [ww[1], -ww[0]]
        ww2 = [-ww[1], ww[0]]
        self.line, = self.ax.plot([ww1[0], ww2[0]], [ww1[1], ww2[1]], "--k")
        self.fig.canvas.draw()

        sleep(2)
        for i, w in enumerate(self.perceptron.getHistory()):
            self.ax.set_title("iteration {0}".format(i))
            sleep(2)
            n = norm(w)
            ww = w / n
            ww1 = [ww[1], -ww[0]]
            ww2 = [-ww[1], ww[0]]
            self.line.set_data([ww1[0], ww2[0]], [ww1[1], ww2[1]])
            self.fig.canvas.draw()

        self.ax.set_title("the algorithm converged in {0} iterations".format(len(self.perceptron.getHistory()) - 1))
        self.fig.canvas.draw()

def nrms(data_fit, data_true):
    """
    Normalized root mean square error.
    """
    # root mean square error
    rms = pl.mean(pl.norm(data_fit - data_true, axis=0))

    # normalization factor is the max - min magnitude, or 2 times max dist from mean
    norm_factor = 2*pl.norm(data_true - pl.mean(data_true, axis=1), axis=0).max()
    return (norm_factor - rms)/norm_factor

def lag_vector( vector_path, p=2 ):
    """
    vector_path : path to lag vector on disk, 

    eg., /sciclone/data10/jberwald/RBC/cells/persout/old_8_pdist_lag1.npy

    p : int or 'inf'
    """
    vec = np.load( vector_path )
    if p == 'inf':
        vecnorm = norm( vec, ord=np.inf )
    else:
        vecnorm = norm( vec, ord=p )
    return vecnorm

    def simulate(self, f_u, x0, tf):
        """
        Simulate the system.

        Parameters
        ----------
        f_u: The input function  f_u(t, x, i)
        x0: The initial state.
        tf: The final time.

        Return
        ------
        data : A StateSpaceDataArray object.

        """
        #pylint: disable=too-many-locals, no-member
        x0 = pl.matrix(x0)
        assert x0.shape[1] == 1
        t = 0
        x = x0
        dt = self.dt
        data = StateSpaceDataList([], [], [], [])
        i = 0
        n_x = self.A.shape[0]
        n_y = self.C.shape[0]
        assert pl.matrix(f_u(0, x0, 0)).shape[1] == 1
        assert pl.matrix(f_u(0, x0, 0)).shape[0] == n_y

        # take square root of noise cov to prepare for noise sim
        if pl.norm(self.Q) > 0:
            sqrtQ = scipy.linalg.sqrtm(self.Q)
        else:
            sqrtQ = self.Q

        if pl.norm(self.R) > 0:
            sqrtR = scipy.linalg.sqrtm(self.R)
        else:
            sqrtR = self.R

        # main simulation loop
        while t + dt < tf:
            u = f_u(t, x, i)
            v = sqrtR.dot(pl.randn(n_y, 1))
            y = self.measurement(x, u, v)
            data.append(t, x, y, u)
            w = sqrtQ.dot(pl.randn(n_x, 1))
            x = self.dynamics(x, u, w)
            t += dt
            i += 1
        return data.to_StateSpaceDataArray()

def angle(v1,v2):
    v1 = array(v1)
    v2 = array(v2)
    val = dot(v1,v2)/float(norm(v1)*norm(v2))
    while val<-1:
        val += 2
    
    while val>1:
        val -= 2
        
        
    a = acos(val)
    a = a*180/pi
    return a

def snr(x, y):
    """
    snr - signal to noise ratio

       v = snr(x,y);

     v = 20*log10( norm(x(:)) / norm(x(:)-y(:)) )

       x is the original clean signal (reference).
       y is the denoised signal.

    Copyright (c) 2014 Gabriel Peyre
    """

    return 20 * np.log10(pylab.norm(x) / pylab.norm(x - y))

    def action(self):
        direction = self.robot.getAngle()#direction es angulo en el campo del robot
        posicion = self.robot.getVel()#hacia donde apunta 
        posicionP = self.pelota.getPos()-self.robot.getPos()#donde se encuentra la pelota relativo al robot
#        print distance
#        print direction        
        
        fr = Front()
        
#        print "robot: P."+str(self.robot.getPos())+" V."+str(posicion)
#        print "pelota:"+str(self.pelota.getPos())+" PR."+str(posicionP)

        angleb = angle(posicion,[posicionP[0],posicionP[1]])#calculo el angulo entre ellos
#        print "angulo entre ellos:"+str(angleb)
#        print "direction:"+str(direction)
        vectorp = norm(posicionP)*array([cos((direction+angleb)/180.0*pi),-sin((direction+angleb)/180.0*pi)])#supongo que el angulo se mide hacia la izquierda
        #vuelvo a calcular un vector supuesto que tenga la misma direccion
#        print "nuevo:"+str(vectorp)+" compar:"+str(posicionP)
        vectorp = vectorp-posicionP#calculo la diferencia de valores
        if norm(vectorp)>10:
            #quiere decir que esta medido a la derecha
            angleb = -angleb
        
        fr.setTR(fr.TR(angleb))
        fr.setST(fr.ST(angleb))
        fr.setTL(fr.TL(angleb))
        
#        i1 = []
#        for i in range(-90,90,5):
#            i1.append(fr.evalFunc(i))
#            
#        plot([i for i in range(-90,90,5)],i1)
#        show()
#        
#        
        
        val = integrate(lambda x:fr.evalFuncUp(x),-45,45)
        if val!=0:
            val = val/integrate(lambda x:fr.evalFunc(x),-45,45)
            

        print "Cambio de angulo:"+str(val)
        print "Angulo o:"+str(self.robot.getAngle())
        self.robot.addAngle(val)
        self.robot.move()
    
        print "Robot:"+str(self.robot.getPos())
        print "Pelota:"+str(self.pelota.getPos())

def main():
    inputs = np.array([[0, 0], [0, 1], [1, 0], [1, 1]])
    targets = np.array([[0], [1], [1], [1]])

    p = Perceptron(inputs, targets)
    p.fit()

    print '--- predict phase ---'
    inputs_bias = np.concatenate((-np.ones((inputs.shape[0], 1)), inputs), axis=1)
    print p.predict(inputs_bias)

    print '\n'
    inputs2, targets2 = gen_data(20)
    p2 = Perceptron(inputs2, targets2)
    p2.fit()

    print '\n--- predict phase ---'
    test_inputs2, test_targets2 = gen_data(10)
    test_inputs_bias2 = np.concatenate((-np.ones((test_inputs2.shape[0], 1)), test_inputs2), axis=1)
    print p2.predict(test_inputs_bias2)

    for i, x in enumerate(test_inputs2):
        if test_targets2[i][0] == 1:
            plt.plot(x[0], x[1], 'ob')
        else:
            plt.plot(x[0], x[1], 'or')

    n = norm(p2.w)
    ww = p2.w / n
    ww1 = [ww[1], -ww[0]]
    ww2 = [-ww[1], ww[0]]
    plt.plot([ww1[0], ww2[0]], [ww1[1], ww2[1]], '--k')
    plt.show()

def VectorToroidalField(TF):
	Nij = 25
#	x = linspace(0,1.4,Nij)
#	y = linspace(0,1.4,Nij)
#	x = linspace(1.0,1.4,Nij,float)
	x = linspace(0.9,RInj[0],Nij,float)
	y = linspace(-0.4,0.4,Nij,float)

	X=[]; Y=[]; Bx=[]; By=[]; Mag=[];
	for i in range(Nij):
		#print i
		for j in range(Nij):
			R = array( [x[i] , y[j], 0.0 ])
			B = TF.local(R)
			X.append(x[i]);
			Y.append(y[j]);
			Bx.append(B[0])
			By.append(B[1]);
			MAG = sqrt(B[0]**2 + B[1]**2 + B[2]**2)
			if (MAG < 0.8 and pl.norm(R)<1.0) or (MAG < 0.8):
				Mag.append( MAG )
			else:
				Mag.append( nan )
	Q = pl.quiver( X , Y , Bx, By , array(Mag) , pivot='mid', scale=10, width =0.005,cmap=mpl.cm.winter) #,cmap=mpl.cm.winter
#	pl.title(r'Toroidal B($r,\phi$)-field (uniform in $z$)')
	pl.title(r'Toroidal Field Map (Top View) B($r,\phi$)/|B$(R_o,\phi)$|')
	pl.xlabel('x [m]'); pl.ylabel('y [m]');
	cb=pl.colorbar();
#	pl.xlim(min(x),max(x));pl.ylim(min(y),max(y))
	pl.xlim(min(x),RInj[0]);pl.ylim(min(y),max(y))
	return X,Y,Bx,By,Mag

    def _get_angles(steps,track_length):
        angles = pl.zeros(track_length-2)
        polar = pl.zeros(pl.shape(steps))
        for i in range(track_length-1):
            polar[i,0] = pl.norm(steps[i,:])
            polar[i,1] = pl.arctan(steps[i,0]/steps[i,1])

            if pl.isnan( polar[i,1]):
                polar[i,1] = 0

            if (steps[i,0] >= 0):
                if (steps[i,1] >= 0):
                    pass
                elif (steps[i,1] < 0):
                    polar[i,1] += 2.*pl.pi
            elif (steps[i,0] < 0):
                if (steps[i,1] >= 0):
                    polar[i,1] += pl.pi
                elif (steps[i,1] < 0):
                    polar[i,1] += pl.pi

        for i in range(track_length-2):
            angles[i] = polar[i+1,1] - polar[i,1]

        return angles

    def rhs(self, z, t=0.):
        """ this function represents the system
        """
        # falls endliche fluchtzeit:
        # abfrage ob norm(x)>10**5
        norm_z = pl.norm(z)

        if norm_z > self.max_norm:
            myLogger.debug_message("norm(z) exceeds " + str(self.max_norm) + ": norm(z) = " + str(norm_z))
            z2 = (z / norm_z) * self.max_norm
            self.x, self.y = z2
        else:
            self.x, self.y = z

        xx_dot = self.x_dot(self.x, self.y)
        yy_dot = self.y_dot(self.x, self.y)

        zDot = xx_dot, yy_dot

        #         norm_zDot = norm(zDot)
        #
        #         if norm_zDot>self.max_norm*1e3:
        #             myLogger.debug_message("norm(z dot) exceeds 1e10: norm(z')="+str(norm_zDot))

        return np.array([xx_dot, yy_dot])

def find_convex_hull(X, num_iter, num_points=None):
    """
        if num_points is set to None, find_convex_hull will return all the points in
        the convex hull (that have been found) sorted according to their sharpness.
        Otherwise, it will return the N-sharpest points.
    """
    (N, D) = X.shape
    if (num_points == None):
        num_points = N

    # randomly choose 'num_iter' direction on the unit sphere.
    # find the maximal point in the chosen direction, and add 1 to its counter.
    # only points on the convex hull will be hit, and 'sharp' corners will 
    # have more hits than 'smooth' corners.
    hits = p.zeros((N, 1))
    for j in xrange(num_iter):
        a = p.randn(D)
        a = a / p.norm(a)
        i = p.dot(X, a).argmax()
        hits[i] += 1
    
    # don't take points with 0 hits
    num_points = min(num_points, sum(p.find(hits)))
    
    # the indices of the n-best points
    o = list(p.argsort(hits, 0)[xrange(-1, -(num_points+1), -1)].flat)
    
    return X[o, :]