def trilaterate3D(distances2):
    p1=np.array(distances2[0][:3])
    p2=np.array(distances2[1][:3])
    p3=np.array(distances2[2][:3])       
    p4=np.array(distances2[3][:3])
    r1=distances2[0][-1]
    r2=distances2[1][-1]
    r3=distances2[2][-1]
    r4=distances2[3][-1]
    e_x=(p2-p1)/np.linalg.norm(p2-p1)
    i=np.dot(e_x,(p3-p1))
    e_y=(p3-p1-(i*e_x))/(np.linalg.norm(p3-p1-(i*e_x)))
    e_z=np.cross(e_x,e_y)
    d=np.linalg.norm(p2-p1)
    j=np.dot(e_y,(p3-p1))
    x=((r1**2)-(r2**2)+(d**2))/(2*d)
    y=(((r1**2)-(r3**2)+(i**2)+(j**2))/(2*j))-((i/j)*(x))
    z1=np.sqrt(r1**2-x**2-y**2)
    z2=np.sqrt(r1**2-x**2-y**2)*(-1)
    ans1=p1+(x*e_x)+(y*e_y)+(z1*e_z)
    ans2=p1+(x*e_x)+(y*e_y)+(z2*e_z)
    dist1=np.linalg.norm(p4-ans1)
    dist2=np.linalg.norm(p4-ans2)
    if np.abs(r4-dist1)<np.abs(r4-dist2):
        return ans1
    else: 
        return ans2

def check_vpd_ks2_astrometry():
    """
    Check the VPD and quiver plots for our KS2-extracted, re-transformed astrometry.
    """
    catFile = workDir + '20.KS2_PMA/wd1_catalog.fits'
    tab = atpy.Table(catFile)

    good = (tab.xe_160 < 0.05) & (tab.ye_160 < 0.05) & \
        (tab.xe_814 < 0.05) & (tab.ye_814 < 0.05) & \
        (tab.me_814 < 0.05) & (tab.me_160 < 0.05)

    tab2 = tab.where(good)

    dx = (tab2.x_160 - tab2.x_814) * ast.scale['WFC'] * 1e3
    dy = (tab2.y_160 - tab2.y_814) * ast.scale['WFC'] * 1e3

    py.clf()
    q = py.quiver(tab2.x_814, tab2.y_814, dx, dy, scale=5e2)
    py.quiverkey(q, 0.95, 0.85, 5, '5 mas', color='red', labelcolor='red')
    py.savefig(workDir + '20.KS2_PMA/vec_diffs_ks2_all.png')

    py.clf()
    py.plot(dy, dx, 'k.', ms=2)
    lim = 30
    py.axis([-lim, lim, -lim, lim])
    py.xlabel('Y Proper Motion (mas)')
    py.ylabel('X Proper Motion (mas)')
    py.savefig(workDir + '20.KS2_PMA/vpd_ks2_all.png')

    idx = np.where((np.abs(dx) < 10) & (np.abs(dy) < 10))[0]
    print('Cluster Members (within dx < 10 mas and dy < 10 mas)')
    print(('   dx = {dx:6.2f} +/- {dxe:6.2f} mas'.format(dx=dx[idx].mean(),
                                                        dxe=dx[idx].std())))
    print(('   dy = {dy:6.2f} +/- {dye:6.2f} mas'.format(dy=dy[idx].mean(),
                                                        dye=dy[idx].std())))

def cada_torrilhon_limiter(r,cfl,epsilon=1.0e-3):
    r"""
    Cada-Torrilhon modified
    
    Additional Input:
     - *epsilon* = 
    """
    a = np.ones((2,len(r))) * 0.95
    b = np.empty((3,len(r)))

    a[0,:] = cfl
    cfl = np.min(a)
    a[1,:] = 0.05
    cfl = np.max(a)
    
    # Multiply all parts except b[0,:] by (1.0 - epsilon) as well
    b[0,:] = 1.0 + (1+cfl) / 3.0 * (r - 1)
    b[1,:] = 2.0 * np.abs(r) / (cfl + epsilon)
    b[2,:] = (8.0 - 2.0 * cfl) / (np.abs(r) * (cfl - 1.0 - epsilon)**2)
    b[1,::2] *= (1.0 - epsilon)
    a[0,:] = np.min(b)
    a[1,:] = (-2.0 * (cfl**2 - 3.0 * cfl + 8.0) * (1.0-epsilon)
                    / (np.abs(r) * (cfl**3 - cfl**2 - cfl + 1.0 + epsilon)))
    
    return np.max(a)

def diamond(radius, dtype=np.uint8):
    """
    Generates a flat, diamond-shaped structuring element of a given
    radius.  A pixel is part of the neighborhood (i.e. labeled 1) if
    the city block/manhattan distance between it and the center of the
    neighborhood is no greater than radius.

    Parameters
    ----------
    radius : int
        The radius of the diamond-shaped structuring element.

    Other Parameters
    ----------------
    dtype : data-type
        The data type of the structuring element.

    Returns
    -------

    selem : ndarray
        The structuring element where elements of the neighborhood
        are 1 and 0 otherwise.
    """
    half = radius
    (I, J) = np.meshgrid(range(0, radius * 2 + 1), range(0, radius * 2 + 1))
    s = np.abs(I - half) + np.abs(J - half)
    return np.array(s <= radius, dtype=dtype)

    def zplane(self, title="", fontsize=18):
        """ Display filter in the complex plane

        Parameters
        ----------

        """
        rb = self.z
        ra = self.p

        t = np.arange(0, 2 * np.pi + 0.1, 0.1)
        plt.plot(np.cos(t), np.sin(t), "k")

        plt.plot(np.real(ra), np.imag(ra), "x", color="r")
        plt.plot(np.real(rb), np.imag(rb), "o", color="b")
        M1 = -10000
        M2 = -10000
        if len(ra) > 0:
            M1 = np.max([np.abs(np.real(ra)), np.abs(np.imag(ra))])
        if len(rb) > 0:
            M2 = np.max([np.abs(np.real(rb)), np.abs(np.imag(rb))])
        M = 1.6 * max(1.2, M1, M2)
        plt.axis([-M, M, -0.7 * M, 0.7 * M])
        plt.title(title, fontsize=fontsize)
        plt.show()

def getOmega(dels):    
#    for k in range(1,dels.delta_d.shape[0])
    N = dels.delta_d.shape[1]
    delta_t = dels.delta_t
    delta_d = dels.delta_d
    
    a_t = np.diff(delta_t)
    a_t = a_t[:,0:-1]
    
    a_d = np.diff(delta_t[:,::-1])
    a_d = a_d[:,::-1]
    a_d = a_d[:,1::]
    
    b_t = np.diff(delta_d)
    b_t = b_t[:,0:-1]
    
    b_d = np.diff(delta_d[:,::-1])
    b_d = b_d[:,::-1]
    b_d = b_d[:,1::]    
    
    c_t = 0.25*(np.abs(a_t)+np.abs(b_t))*np.sign(a_t)*np.sign(b_t)*(np.sign(a_t)*np.sign(b_t)-1)
    c_d = 0.25*(np.abs(a_d)+np.abs(b_d))*np.sign(a_d)*np.sign(b_d)*(np.sign(a_d)*np.sign(b_d)-1)
    Omega = 1.0/(2*N)*(c_t.mean(axis=0) + c_d.mean(axis=0))

    return Omega

    def test_surface_evaluate(self):
        from sfepy.discrete import FieldVariable
        problem = self.problem

        us = problem.get_variables()['us']
        vec = nm.empty(us.n_dof, dtype=us.dtype)
        vec[:] = 1.0
        us.set_data(vec)

        expr = 'ev_surface_integrate.i.Left( us )'
        val = problem.evaluate(expr, us=us)
        ok1 = nm.abs(val - 1.0) < 1e-15
        self.report('with unknown: %s, value: %s, ok: %s'
                    % (expr, val, ok1))

        ps1 = FieldVariable('ps1', 'parameter', us.get_field(),
                            primary_var_name='(set-to-None)')
        ps1.set_data(vec)

        expr = 'ev_surface_integrate.i.Left( ps1 )'
        val = problem.evaluate(expr, ps1=ps1)
        ok2 = nm.abs(val - 1.0) < 1e-15
        self.report('with parameter: %s, value: %s, ok: %s'
                    % (expr, val, ok2))
        ok2 = True

        return ok1 and ok2

def evolve(part, logLstar):
	q = part.params2q(part.params)	# normalized array of thawed parameters
	n = len(q)
	qprime = copy.copy(q)
	m = g.masses[g.thawedIdxs]

	for t in range(g.T):		
		dq = part.stepsize * np.random.randn(n) / m
		dq[np.abs(dq)>g.maxStep] = g.maxStep
		dq[np.abs(dq)<g.minStep] = g.minStep

		qprime += dq

		# bounce if out of bounds:
		if part.outOfBounds(qprime):
			qprime = bounce(qprime)
		
		# check likelihood constraint:
		logL = part.measLoglike(qprime)
		if  logL < logLstar:
			part.reject()
			#print (logL, 'r', part.stepsize, qprime)
		else:
			part.accept(qprime)
			part.distance += np.linalg.norm(dq)
			#print (logL, 'a', part.stepsize, qprime)

	print '\ntraveled %.6f \n' % part.distance

def _gpinv(p, k, sigma):
    """Inverse Generalized Pareto distribution function"""
    x = np.full_like(p, np.nan)
    if sigma <= 0:
        return x
    ok = (p > 0) & (p < 1)
    if np.all(ok):
        if np.abs(k) < np.finfo(float).eps:
            x = - np.log1p(-p)
        else:
            x = np.expm1(-k * np.log1p(-p)) / k
        x *= sigma
    else:
        if np.abs(k) < np.finfo(float).eps:
            x[ok] = - np.log1p(-p[ok])
        else:
            x[ok] = np.expm1(-k * np.log1p(-p[ok])) / k
        x *= sigma
        x[p == 0] = 0
        if k >= 0:
            x[p == 1] = np.inf
        else:
            x[p == 1] = - sigma / k

    return x

    def connect_edges(self):
        """Connect detected edges based on their slopes."""
        # Fitting a straight line to each edge.
        p0 = [0., 0.]
        radian2angle = 180. / np.pi
        for edge in self.edges:
            p1, s = leastsq(self.residuals, p0,
                            args=(edge['x'][:-1], edge['y'][:-1]))
            edge['slope'] = p1[0]
            edge['intercept'] = p1[1]
            edge['slope_angle'] = np.arctan(edge['slope']) * radian2angle

        # Connect by the slopes of two edges.
        len_edges = len(self.edges)
        for i in range(len_edges - 1):
            for j in range(i + 1, len_edges):
                if np.abs(self.edges[i]['slope_angle'] -
                          self.edges[j]['slope_angle']) <= \
                   self.connectivity_angle:
                    # Then, the slope between the centers of the two edges
                    # should be similar with the slopes of
                    # the two lines of the edges as well.
                    c_slope = (self.edges[i]['y_center'] -
                                    self.edges[j]['y_center']) / \
                                   (self.edges[i]['x_center'] -
                                    self.edges[j]['x_center'])
                    c_slope_angle = np.arctan(c_slope) * radian2angle

                    if np.abs(c_slope_angle - self.edges[i]['slope_angle']) <= \
                       self.connectivity_angle and \
                       np.abs(c_slope_angle - self.edges[j]['slope_angle']) <= \
                       self.connectivity_angle:
                        self.edges[i]['connectivity'] = self.edges[j]['index']
                        break

def t2smap(catd,mask,tes):
	"""
	t2smap(catd,mask,tes)

	Input:

	catd  has shape (nx,ny,nz,Ne,nt)
	mask  has shape (nx,ny,nz)
	tes   is a 1d numpy array
	"""
	nx,ny,nz,Ne,nt = catd.shape
	N = nx*ny*nz

	echodata = fmask(catd,mask)
	Nm = echodata.shape[0]

	#Do Log Linear fit
	B = np.reshape(np.abs(echodata), (Nm,Ne*nt)).transpose()
	B = np.log(B)
	x = np.array([np.ones(Ne),-tes])
	X = np.tile(x,(1,nt))
	X = np.sort(X)[:,::-1].transpose()

	beta,res,rank,sing = np.linalg.lstsq(X,B)
	t2s = 1/beta[1,:].transpose()
	s0  = np.exp(beta[0,:]).transpose()

	#Goodness of fit
	alpha = (np.abs(B)**2).sum(axis=0)
	t2s_fit = blah = (alpha - res)/(2*res)
	
	out = unmask(t2s,mask),unmask(s0,mask),unmask(t2s_fit,mask)

	return out

def plot_robots_ratio_time_micmac(deploy_robots_mic, deploy_robots_mac, deploy_robots_desired, delta_t):
    plot_option = 0 # 0: ratio, 1: cost
    num_iter = deploy_robots_mic.shape[1]
    total_num_robots = np.sum(deploy_robots_mic[:,0,:])
    
    diffmic_sqs = np.zeros(num_iter)
    diffmac_sqs = np.zeros(num_iter)
    diffmic_rat = np.zeros(num_iter)
    diffmac_rat = np.zeros(num_iter)
    for t in range(num_iter):
        diffmic = np.abs(deploy_robots_mic[:,t,:] - deploy_robots_desired)    
        diffmac = np.abs(deploy_robots_mac[:,t,:] - deploy_robots_desired) 
        diffmic_rat[t] = np.sum(diffmic) / total_num_robots       
        diffmic_sqs[t] = np.sum(np.square(diffmic))
        diffmac_rat[t] = np.sum(diffmac) / total_num_robots 
        diffmac_sqs[t] = np.sum(np.square(diffmac))
        
    x = np.arange(0, num_iter) * delta_t
    if(plot_option==0):
        l1 = plt.plot(x,diffmic_rat)
        l2 = plt.plot(x,diffmac_rat)
    if(plot_option==1):
        l1 = plt.plot(x,diffmic_sqs)
        l2 = plt.plot(x,diffmac_sqs)
    
    plt.xlabel('time [s]')    
    plt.ylabel('ratio of misplaced robots')
    plt.legend((l1, l2),('Micro','Macro'))
    plt.show()

  def update(self):
    logging.debug("About to extract features for %d trips " % (len(self.trips)))
    trip_features, labels = self.extract_features()
    logging.debug("trip_features.size() = %s, nTrips = %d " % (trip_features.size, len(self.trips)))
    #TODO: why the hell is this happening..? absurd feature extraction
    trip_features = np.nan_to_num(trip_features)
    trip_features[np.abs(trip_features) < .001] = 0
    trip_features[np.abs(trip_features) > 1000000] = 0
    nonzero = ~np.all(trip_features==0, axis=1)
    logging.debug("nonzero list = %s" % nonzero)
    trip_features = trip_features[nonzero]
    labels = labels[nonzero]

    # logging.debug("Trip Features: %s" % trip_features)
    try:
       self.regression.fit(trip_features, labels)
       self.coefficients = self.regression.coef_
       self.save_coefficients()
    except ValueError as e:
       logging.warning("While fitting the regression, got error %s" % e)
       if ("%s" % e) == "The number of classes has to be greater than one":
          logging.warning("Training set has no alternatives!")
       raise e
       # else:
       #    np.save("/tmp/broken_array", trip_features)
       #    raise e
    '''

def savgol(x, window_size=3, order=2, deriv=0, rate=1):
    ''' Savitzky-Golay filter '''
        
    # Check the input
    try:
        window_size = np.abs(np.int(window_size))
        order = np.abs(np.int(order))
    except ValueError:
        raise ValueError("window_size and order have to be of type int")
    if window_size > len(x):
        raise TypeError("Not enough data points!")
    if window_size % 2 != 1 or window_size < 1:
        raise TypeError("window_size size must be a positive odd number")
    if window_size < order + 1:
        raise TypeError("window_size is too small for the polynomials order")
    if order <= deriv:
        raise TypeError("The 'deriv' of the polynomial is too high.")


    # Calculate some required parameters
    order_range = range(order+1)
    half_window = (window_size -1) // 2
    num_data = len(x)
    
    # Construct Vandermonde matrix, its inverse, and the Savitzky-Golay coefficients   
    a = [[ii**jj for jj in order_range] for ii in range(-half_window, half_window+1)]
    pa = np.linalg.pinv(a)
    sg_coeff = pa[deriv] * rate**deriv * scipy.special.factorial(deriv)
      
    # Get the coefficients for the fits at the beginning and at the end of the data
    coefs = np.array(order_range)**np.sign(deriv)
    coef_mat = np.zeros((order+1, order+1))
    row = 0
    for ii in range(deriv,order+1):
        coef = coefs[ii]
        for jj in range(1,deriv):
            coef *= (coefs[ii]-jj)
        coef_mat[row,row+deriv]=coef
        row += 1
    coef_mat *= rate**deriv
    
    # Add the first and last point half_window times
    firstvals = np.ones(half_window) * x[0] 
    lastvals  = np.ones(half_window) * x[-1]
    x_calc = np.concatenate((firstvals, x, lastvals))

    y = np.convolve( sg_coeff[::-1], x_calc, mode='full')
    
    # chop away intermediate data
    y = y[window_size-1:window_size+num_data-1]

    # filtering for the first and last few datapoints
    y[0:half_window] = np.dot(np.dot(np.dot(a[0:half_window], coef_mat), \
                                   np.mat(pa)), x[0:window_size])
    y[len(y)-half_window:len(y)] = np.dot(np.dot(np.dot(a[half_window+1:window_size], \
                        coef_mat), pa), x[len(x)-window_size:len(x)])
    
    return y

    

def isparallel(O1,O2):
    '''
    Judge whether two array-like vectors are parallel to each other.

    Parameters
    ----------
    O1,O2 : 1d array-like
        The input vectors.

    Returns
    -------
    int
        *  0: not parallel
        *  1: parallel
        * -1: anti-parallel
    '''
    norm1=nl.norm(O1)
    norm2=nl.norm(O2)
    if norm1<RZERO or norm2<RZERO:
        return 1
    elif O1.shape[0]==O2.shape[0]:
        buff=np.inner(O1,O2)/(norm1*norm2)
        if np.abs(buff-1)<RZERO:
            return 1
        elif np.abs(buff+1)<RZERO:
            return -1
        else:
            return 0
    else:
        raise ValueError("isparallel error: the shape of the array-like vectors does not match.")

    def _beam_map_single(self, bl_index, f_index):

        p_stokes = [ 0.5 * np.array([[1.0,   0.0], [0.0,  1.0]]),
                     0.5 * np.array([[1.0,   0.0], [0.0, -1.0]]),
                     0.5 * np.array([[0.0,   1.0], [1.0,  0.0]]),
                     0.5 * np.array([[0.0, -1.0J], [1.0J, 0.0]]) ]

        # Get beam maps for each feed.
        feedi, feedj = self.uniquepairs[bl_index]
        beami, beamj = self.beam(feedi, f_index), self.beam(feedj, f_index)

        # Get baseline separation and fringe map.
        uv = self.baselines[bl_index] / self.wavelengths[f_index]
        fringe = visibility.fringe(self._angpos, self.zenith, uv)

        pow_stokes = [ np.sum(beami * np.dot(beamj.conjugate(), polproj), axis=1) * self._horizon for polproj in p_stokes]

        # Calculate the solid angle of each beam
        pxarea = (4*np.pi / beami.shape[0])

        om_i = np.sum(np.abs(beami)**2 * self._horizon[:, np.newaxis]) * pxarea
        om_j = np.sum(np.abs(beamj)**2 * self._horizon[:, np.newaxis]) * pxarea

        omega_A = (om_i * om_j)**0.5

        # Calculate the complex visibility transfer function
        cv_stokes = [ p * (2 * fringe / omega_A) for p in pow_stokes ]

        return cv_stokes

def plot_marginal_pdfs( res, nbins=101, **kwargs):
    """ plot the results of a classification run
    :return:
    """
    from matplotlib import pyplot as pl
    import numpy as np

    nparam = len(res.vparam_names)
    # nrow = np.sqrt( nparam )
    # ncol = nparam / nrow + 1
    nrow, ncol = 1, nparam

    pdfdict = get_marginal_pdfs( res, nbins )

    fig = pl.gcf()
    for parname in res.vparam_names :
        iax = res.vparam_names.index( parname )+1
        ax = fig.add_subplot( nrow, ncol, iax )

        parval, pdf, mean, std = pdfdict[parname]
        ax.plot(  parval, pdf, **kwargs )
        if np.abs(std)>=0.1:
            ax.text( 0.95, 0.95, '%s  %.1f +- %.1f'%( parname, np.round(mean,1), np.round(std,1)),
                     ha='right',va='top',transform=ax.transAxes )
        elif np.abs(std)>=0.01:
            ax.text( 0.95, 0.95, '%s  %.2f +- %.2f'%( parname, np.round(mean,2), np.round(std,2)),
                     ha='right',va='top',transform=ax.transAxes )
        elif np.abs(std)>=0.001:
            ax.text( 0.95, 0.95, '%s  %.3f +- %.3f'%( parname, np.round(mean,3), np.round(std,3)),
                     ha='right',va='top',transform=ax.transAxes )
        else :
            ax.text( 0.95, 0.95, '%s  %.3e +- %.3e'%( parname, mean, std),
                     ha='right',va='top',transform=ax.transAxes )

    pl.draw()

def max_lm(baselines, wavelengths, uwidth, vwidth=0.0):
    """Get the maximum (l,m) that a baseline is sensitive to.

    Parameters
    ----------
    baselines : np.ndarray
        An array of baselines.
    wavelengths : np.ndarray
        An array of wavelengths.
    uwidth : np.ndarray
        Width of the receiver in the u-direction.
    vwidth : np.ndarray
        Width of the receiver in the v-direction.

    Returns
    -------
    lmax, mmax : array_like
    """

    umax = (np.abs(baselines[:, 0]) + uwidth) / wavelengths
    vmax = (np.abs(baselines[:, 1]) + vwidth) / wavelengths

    mmax = np.ceil(2 * np.pi * umax).astype(np.int64)
    lmax = np.ceil((mmax**2 + (2*np.pi*vmax)**2)**0.5).astype(np.int64)

    return lmax, mmax

def compare_objs(x, y):
    assert type(x) is type(y)
    if type(x) is dict:
        assert x.keys().sort() == y.keys().sort()
        for ky in x:
            compare_objs(x[ky], y[ky])
    elif type(x) is list:
        assert len(x) == len(y)
        for ind in range(len(x)):
            compare_objs(x[ind], y[ind])
    elif type(x) is np.ndarray:
        assert x.shape == y.shape
        if not np.allclose(x, y, atol=1.0e-5, rtol=0.0):
            x = x.reshape(x.size)
            y = y.reshape(y.size)
            dd = x - y
            worst_case = np.max(np.abs(dd))
            print "worst case abs diff = %e" % worst_case
            ind = np.where((x != 0) | (y != 0))
            rel_err = np.abs(np.divide(dd[ind], np.abs(x[ind]) + np.abs(y[ind])))
            worst_case = np.max(rel_err)
            print "worst case rel diff = %e" % worst_case
            assert False
    else:
        assert x == y

    def DM(self, z):
        """Transverse Comoving Distance (Mpc)

        Parameters
        ----------
        z : float
            redshift
        
        Returns
        -------
        y : float
            The transverse comoving distance in Mpc, given by Hogg eqn 16
            
        Examples
        --------
        >>> cosmo = Cosmology()
        >>> cosmo.DM(1.0)
        3303.8288058874678
        """
        # Compute the transverse comoving distance in Mpc (Eqn 16)
        if self.OmegaK > 0.0:
            return self.DH / np.sqrt(self.OmegaK) * \
                    np.sinh(np.sqrt(self.OmegaK)*self.DC(z)/self.DH)
        elif self.OmegaK == 0.0:
            return self.DC(z)
        elif self.OmegaK < 0.0:
            return self.DH / np.sqrt(np.abs(self.OmegaK)) * \
                    np.sin(np.sqrt(np.abs(self.OmegaK))*self.DC(z)/self.DH)