def clockwise_angle(x,y,r=None,r0=None):
  # angle between two vectors defined by three points,
  #(x0,y0),(x1,y1),(x2,y2)

  x1=x[0]-x[1]
  x2=x[2]-x[1]

  y1=y[0]-y[1]
  y2=y[2]-y[1]

  dot = x1*x2 + y1*y2
  det = x1*y2 - y1*x2
  angle = np.arctan2(det, dot)

  if angle<0: angle=-angle
  else: angle=2*np.pi-angle

  if r and r==r0: # may not make sense if r~=r0
    L=np.sqrt(x2**2+y2**2)
    a=np.arccos(L/r)
    angle=angle-a

    L0=np.sqrt(x1**2+y1**2)
    if L0<r0:
      a0=np.arccos(L0/r0)
      angle=angle-a0

  return angle

	def solve_nonlinear(self, params, unknowns, resids):
		
		x = params['xr']
		y = params['yr']
		z = params['z']
		r = params['r']
		alpha = params['alpha']
		nTurbs = len(x)
		
		overlap_fraction = np.eye(nTurbs)
		for i in range(nTurbs):
			for j in range(nTurbs): #overlap_fraction[i][j] is the fraction of the area of turbine i in the wake from turbine j
				dx = x[i]-x[j]
				dy = abs(y[i]-y[j])
				dz = abs(z[i]-z[j])
				d = np.sqrt(dy**2+dz**2)
				R = r[j]+dx*alpha
				A = r[i]**2*np.pi
				overlap_area = 0
				if dx <= 0: #if turbine i is in front of turbine j
					overlap_fraction[i][j] = 0.0
				else:
					if d <= R-r[i]: #if turbine i is completely in the wake of turbine j
						if A <= np.pi*R**2: #if the area of turbine i is smaller than the wake from turbine j
							overlap_fraction[i][j] = 1.0
						else: #if the area of turbine i is larger than tha wake from turbine j
							overlap_fraction[i][j] = np.pi*R**2/A
					elif d >= R+r[i]: #if turbine i is completely out of the wake
						overlap_fraction[i][j] = 0.0
					else: #if turbine i overlaps partially with the wake
						overlap_area = r[i]**2.*np.arccos((d**2.+r[i]**2.-R**2.)/(2.0*d*r[i]))+R**2.*np.arccos((d**2.+R**2.-r[i]**2.)/(2.0*d*R))-0.5*np.sqrt((-d+r[i]+R)*(d+r[i]-R)*(d-r[i]+R)*(d+r[i]+R))
						overlap_fraction[i][j] = overlap_area/A
				
		print "Overlap Fraction Matrix: ", overlap_fraction
		unknowns['overlap'] = overlap_fraction

        def get_new_cell(self):
            """Returns new basis vectors"""
            a = np.sqrt(self.a)
            b = np.sqrt(self.b)
            c = np.sqrt(self.c)

            ad = self.atoms.cell[0] / np.linalg.norm(self.atoms.cell[0])

            Z = np.cross(self.atoms.cell[0], self.atoms.cell[1])
            Z /= np.linalg.norm(Z)
            X = ad - np.dot(ad, Z) * Z
            X /= np.linalg.norm(X)
            Y = np.cross(Z, X)

            alpha = np.arccos(self.x / (2 * b * c))
            beta = np.arccos(self.y / (2 * a * c))
            gamma = np.arccos(self.z / (2 * a * b))

            va = a * np.array([1, 0, 0])
            vb = b * np.array([np.cos(gamma), np.sin(gamma), 0])
            cx = np.cos(beta)
            cy = (np.cos(alpha) - np.cos(beta) * np.cos(gamma)) \
                / np.sin(gamma)
            cz = np.sqrt(1. - cx * cx - cy * cy)
            vc = c * np.array([cx, cy, cz])

            abc = np.vstack((va, vb, vc))
            T = np.vstack((X, Y, Z))
            return np.dot(abc, T)

def star(a,b,c,alpha,beta,gamma):
    "Calculate unit cell volume, reciprocal cell volume, reciprocal lattice parameters"
    alpha=np.radians(alpha)
    beta=np.radians(beta)
    gamma=np.radians(gamma)
    V=2*a*b*c*\
        np.sqrt(np.sin((alpha+beta+gamma)/2)*\
               np.sin((-alpha+beta+gamma)/2)*\
               np.sin((alpha-beta+gamma)/2)*\
               np.sin((alpha+beta-gamma)/2))
    Vstar=(2*np.pi)**3/V;
    astar=2*np.pi*b*c*np.sin(alpha)/V
    bstar=2*np.pi*a*c*np.sin(beta)/V
    cstar=2*np.pi*b*a*np.sin(gamma)/V
    alphastar=np.arccos((np.cos(beta)*np.cos(gamma)-\
                        np.cos(alpha))/ \
                       (np.sin(beta)*np.sin(gamma)))
    betastar= np.arccos((np.cos(alpha)*np.cos(gamma)-\
                        np.cos(beta))/ \
                       (np.sin(alpha)*np.sin(gamma)))
    gammastar=np.arccos((np.cos(alpha)*np.cos(beta)-\
                        np.cos(gamma))/ \
                       (np.sin(alpha)*np.sin(beta)))
    V=V
    alphastar=np.degrees(alphastar)
    betastar=np.degrees(betastar)
    gammastar=np.degrees(gammastar)
    return astar,bstar,cstar,alphastar,betastar,gammastar

def Jacobsen(h1, Xm_1, h2, Xm_2):
    alp = np.degrees(np.arccos(np.dot(h1, h2) /
                               (np.linalg.norm(h1) * np.linalg.norm(h2))))
    bet = np.degrees(np.arccos(np.dot(Xm_1, Xm_2) /
                               (np.linalg.norm(Xm_1) * np.linalg.norm(Xm_2))))
    if ((alp - bet)**2) > 1:
        print('check your indexing!')

    a = 3.567  # diamond lattice parameter
    # recip lattice par(note this is the mantid convention: no 2 pi)
    ast = (2 * np.pi) / a
    B = np.array([[ast, 0, 0], [0, ast, 0], [0, 0, ast]])
    Xm_g = np.cross(Xm_1, Xm_2)
    Xm = np.column_stack([Xm_1, Xm_2, Xm_g])

    # Vector Q1 is described in reciprocal space by its coordinate matrix h1
    Xa_1 = B.dot(h1)
    Xa_2 = B.dot(h2)
    Xa_g = np.cross(Xa_1, Xa_2)
    Xa = np.column_stack((Xa_1, Xa_2, Xa_g))

    R = Xa.dot(np.linalg.inv(Xm))
    U = np.linalg.inv(R)

    UB = U.dot(B)

    return UB

def spherical_excess(a, b, c):
    "spherical excess of the triangle."
    A = arccos((cos(a) - cos(b) * cos(c)) / sin(b) / sin(c))
    B = arccos((cos(b) - cos(c) * cos(a)) / sin(c) / sin(a))
    C = arccos((cos(c) - cos(a) * cos(b)) / sin(a) / sin(b))
    E = A + B + C - pi
    return(E)

    def compute_cd(self):
        # DA is the measured sensor value
        # 1) compute diagonal E (using A, D + angle DA)
        # E = sqrt(A * A + D * D - 2 * A * D * cos(DA))
        e = numpy.sqrt(
            self.a * self.a +
            self.d * self.d -
            2 * self.a * self.d * cos(radians(self.da)))

        # 2) compute angle ED of triangle 1 (using A, D, E)
        # ED = acos((E * E + D * D - A * A) / (2 * E * D))
        ed = numpy.arccos(
            (e * e + self.d * self.d - self.a * self.a) /
            (2 * e * self.d))

        # 3) compute angle of CE of triangle 2 (using B, C, E)
        # CE = acos((E * E + C * C - B * B) / (2 * E * C))
        ce = numpy.arccos(
            (e * e + self.c * self.c - self.b * self.b) /
            (2 * e * self.c))

        # 4) add angles #2 and #3
        # CD = CE + ED
        cd = ce + ed  # radians
        #e = numpy.degrees(cd) - self.cd
        #print("calf link angle error: %s" % e)

        # 5) compute excursion of spring using angle #4
        # sqrt(F * F + G * G - 2 * F * G * cos(CD))
        return cd

 def GetOrientTransmat(self, biounit=False, target=False):
     cen = np.zeros((3,1))
     cenlist = self.Centroid(biounit, target)
     cen[0:3] = [[cenlist[0]],[cenlist[1]],[cenlist[2]]]
     tmat = translation_matrix(-self.Centroid(biounit, target))
     max = 0
     farthestxyz = None
     for atom in self.IterAtoms(biounit, target):
         dist = np.sum((atom.xyz[0:3] - cen)**2)
         if dist > max:
             farthestxyz = atom.xyz[0:3] - cen
             max = dist
     firstrotax = np.cross(farthestxyz.transpose().tolist()[0], np.array([1,0,0]))
     firstrotang = np.arccos(np.dot(farthestxyz.transpose().tolist()[0], np.array([1,0,0]))/(np.sqrt(np.dot(farthestxyz.transpose().tolist()[0], farthestxyz.transpose().tolist()[0]))))
     firstrotmat = rotation_matrix(firstrotang, firstrotax, self.Centroid(biounit, target))
     max = 0
     firsttransmat = tmat.dot(firstrotmat)
     for atom in self.IterAtoms(biounit, target):
         dist = sum(firsttransmat.dot(atom.xyz)[1:3]**2)
         if dist > max:
             secfarthestyz = firsttransmat.dot(atom.xyz)[0:3]
             max = dist
     secfarthestyz[0] = 0
     secondrotax = np.array([1,0,0])
     secondrotang = np.arccos(np.dot(secfarthestyz.transpose().tolist()[0], np.array([0,1,0]))/np.sqrt(np.dot(secfarthestyz.transpose().tolist()[0],secfarthestyz.transpose().tolist()[0])))
     secondrotmat = rotation_matrix(secondrotang, secondrotax, self.Centroid(biounit, target))
     return secondrotmat.dot(firsttransmat)

def circle_intersection_area(r, R, d):
    '''
    Formula from: http://mathworld.wolfram.com/Circle-CircleIntersection.html
    Does not make sense for negative r, R or d

    >>> circle_intersection_area(0.0, 0.0, 0.0)
    0.0
    >>> circle_intersection_area(1.0, 1.0, 0.0)
    3.1415...
    >>> circle_intersection_area(1.0, 1.0, 1.0)
    1.2283...
    '''
    if np.abs(d) < tol:
        minR = np.min([r, R])
        return np.pi * minR**2
    if np.abs(r - 0) < tol or np.abs(R - 0) < tol:
        return 0.0
    d2, r2, R2 = float(d**2), float(r**2), float(R**2)
    arg = (d2 + r2 - R2) / 2 / d / r
    arg = np.max([np.min([arg, 1.0]), -1.0])  # Even with valid arguments, the above computation may result in things like -1.001
    A = r2 * np.arccos(arg)
    arg = (d2 + R2 - r2) / 2 / d / R
    arg = np.max([np.min([arg, 1.0]), -1.0])
    B = R2 * np.arccos(arg)
    arg = (-d + r + R) * (d + r - R) * (d - r + R) * (d + r + R)
    arg = np.max([arg, 0])
    C = -0.5 * np.sqrt(arg)
    return A + B + C

        def calculateDiffuseRay(self, intersectionPoint, firstGeometry):

            pointNormal = firstGeometry.getNormal(intersectionPoint)

            r1 = rand.random()
            r2 = rand.random()
            phi = 2 * np.pi * r1
            theta = np.arccos(np.sqrt(r2))

            # To cartesian coordinates
            x = np.cos(phi) * np.sin(theta)
            y = np.sin(phi) * np.sin(theta)
            z = np.cos(theta)
            newDirection = [x, y, z]

            # Rotate new direction to distribution of normal vector
            el = -1 * np.arccos(pointNormal[2])
            az = -1 * np.arctan2(pointNormal[1], pointNormal[0])
            rotationRay = [np.cos(el) * newDirection[0] - np.sin(el) * newDirection[2], newDirection[1], np.sin(el) * newDirection[0] + np.cos(el) * newDirection[2]]
            rotationRay = [np.cos(az) * rotationRay[0] + np.sin(az) * rotationRay[1], -1 * np.sin(az) * rotationRay[0] + np.cos(az) * rotationRay[1], rotationRay[2]]
            newDirectionCorrect = rotationRay / np.linalg.norm(rotationRay)

            randomRay = Ray(newDirectionCorrect, intersectionPoint + (0.00001 * newDirectionCorrect))

            return randomRay

def miso((q1, q2)):
    q1 = quaternion.Quaternion(numpy.array(q1) / numpy.linalg.norm(q1)).conjugate()
    q2 = quaternion.Quaternion(numpy.array(q2) / numpy.linalg.norm(q2)).conjugate()
    misot = 180.0
    misoa = None

    for i in range(len(cubicSym)):
        for ii in range(len(orthoSym)):
            qa = orthoSym[ii] * q1 * cubicSym[i]

            for j in range(len(cubicSym)):
                #for jj in range(len(orthoSym)):
                    qb = q2 * cubicSym[j]

                    qasb1 = qa.conjugate() * qb
                    qasb2 = qb * qa.conjugate()

                    t1 = qasb1.wxyz / numpy.linalg.norm(qasb1.wxyz)
                    t2 = qasb2.wxyz / numpy.linalg.norm(qasb2.wxyz)

                    a1 = 2 * numpy.arccos(t1[0]) * 180 / numpy.pi
                    a2 = 2 * numpy.arccos(t2[0]) * 180 / numpy.pi

                    if a1 < misot:
                        misot = a1
                        misoa = qasb1

                    if a2 < misot:
                        misot = a2
                        misoa = qasb2

    return misot

    def overlap(self, blob):
        """Overlap between two blobs.
        
        Defined by the overlap area.
        """
        # For now it is just the overlap area of two containment circles
        # It could be replaced by the Q or C factor, which also defines
        # a certain neighborhood.

        d = sqrt((self.x_pos - blob.x_pos) ** 2 + (self.y_pos - blob.y_pos) ** 2)

        # One circle lies inside the other
        if d < abs(self.radius - blob.radius):
            area = pi * min(self.radius, blob.radius) ** 2

        # Circles don't overlap
        elif d > (self.radius + blob.radius):
            area = 0

        # Compute overlap area.
        # Reference: http://mathworld.wolfram.com/Circle-CircleIntersection.html (04.04.2013)
        else:
            term_a = blob.radius ** 2 * arccos((d ** 2 + blob.radius ** 2 - self.radius ** 2) / (2 * d * blob.radius))
            term_b = self.radius ** 2 * arccos((d ** 2 + self.radius ** 2 - blob.radius ** 2) / (2 * d * self.radius))
            term_c = 0.5 * sqrt(
                abs(
                    (-d + self.radius + blob.radius)
                    * (d + self.radius - blob.radius)
                    * (d - self.radius + blob.radius)
                    * (d + self.radius + blob.radius)
                )
            )
            area = term_a + term_b - term_c

        return max(area / self.area(), area / blob.area())

 def get_sunset_ele(d):
     if (-np.tan(get_declination_angle(d)) * np.tan(np.deg2rad(lat))) > 1.0:
         return np.arccos(1.0)
     elif (-np.tan(get_declination_angle(d)) * np.tan(np.deg2rad(lat))) < -1.0:
         return np.arccos(-1.0)
     else:
         return np.arccos(-np.tan(get_declination_angle(d)) * np.tan(np.deg2rad(lat)))

def main():
    map_obstacles = np.loadtxt('obstacles_map.txt')
    laser_obstacles = np.loadtxt('obstacles_laser.txt')

    true_rotation = np.pi / 10
    true_translation = np.array([5, 5])

    laser_rot = rotate(laser_obstacles, true_rotation)
    laser_trans = translate(laser_rot, true_translation)

    t, r = relocalize(map_obstacles, laser_rot)
    theta = np.arccos(r.item(0, 1))

    print "True Rotation:", true_rotation
    print "True Translation:", true_translation
    print "-------------------------------------"
    print "Estimated Rotation:", theta
    print "Estimated Translation:", t
    print "-------------------------------------"
    print "Rotation Error:", np.abs(true_rotation - theta)
    print "Translation Error:", np.abs(true_translation - t)

    laser_reloc = rotate(laser_rot, -np.arccos(r.item(0, 1)))

    plot_super(map_obstacles, laser_obstacles, "Original")
    plot_super(map_obstacles, laser_trans, "Measure misaligned with map")
    plot_super(map_obstacles, laser_reloc, "Measure realigned with map")
    plt.show()

def PlotEccOrbit_aRs(par,t):
  """Function to plot planet orbit in 3D"""
  
  #read in parameters
  T0,P,a_Rstar,p,b,c1,c2,e,w,foot,Tgrad,Sec_depth = par

  #ensure b and p >= 0
  if b<0.: b=-b
  if p<0.: p=-p

  w *= np.pi / 180.
  i = np.arccos(b/a_Rstar)
  
  #make w lie in range 0-2pi
  if w >= 2*np.pi:
    w -= 2*np.pi #make f lie in range 0-2pi
  elif w < 0:
    w += 2*np.pi #make f lie in range 0-2pi

  #true anomaly of central transit time
  f = 1.*np.pi/2. + w
  if f >= 2*np.pi:
    f -= 2*np.pi #make f lie in range 0-2pi
  elif f < 0:
    f += 2*np.pi #make f lie in range 0-2pi
  
  if f < np.pi:
    E = np.arccos( (np.cos(f) + e) / (e*np.cos(f)+1.) )
    M_tr = E - e*np.sin(E)
    T_peri = T0 + M_tr * P/(2*np.pi)

  if f >= np.pi:
    #f = np.pi - f #correct for acos calc
    E = np.arccos( (np.cos(f) + e) / (e*np.cos(f)+1.) )
    M_tr = E - e*np.sin(E)
    #M_tr = 2*np.pi - M_tr
    T_peri = T0 - M_tr * P/(2*np.pi)

  #calculate mean anomaly
  M = (2*np.pi/P) * (t - T_peri)
  
  #get coords
  x = PlanetOrbit.get_x(M,a_Rstar,e,w)
  y = PlanetOrbit.get_y(M,a_Rstar,e,w,i)
  z = PlanetOrbit.get_z(M,a_Rstar,e,w,i)
  
  #make plot
  ax = Axes3D(pylab.gcf())
  ax.plot(x, y, z, c='k')
  ax.scatter(x, y, z, c='r', s=50)
  ax.scatter([0],[0],[0],c='y', s=500) #plot star position
  ax.scatter([x[0],],[y[0],],[z[0],],c='g', s=100) #plot initial planet position
  ax.scatter([x[1],],[y[1],],[z[1],],c='y', s=100) #plot initial planet position
  ax.set_xlabel('X')
  ax.set_ylabel('Y')
  ax.set_zlabel('Z')
  range = abs(np.array([x,y,z])).max()
  ax.set_xlim3d(-range,range)
  ax.set_ylim3d(-range,range)
  ax.set_zlim3d(-range,range)

def draw_cell_2d(axis, cell_output, total_radius=True, zorder=0, y_limits=None, alpha=1.0):
    """

    """
    (x, y, z) = cell_output.get_location()
    rad = cell_output.get_radius(total_radius=total_radius)
    if cell_output.color == None:
        print 'Cell has no defined color!'
        col = (0, 1, 0)
    else:
        col = cell_output.color
    #col = (0, 1, 0) if cell_output.color == None else cell_output.color
    #col = cell_output.color
    if (y_limits != None) and (y - rad < y_limits[0]):
        segment = toolbox_schematic.CircleSegment()
        segment.set_defaults(alpha=alpha, edgecolor='none', facecolor=col, zorder=zorder)
        angle = pi - numpy.arccos((y - y_limits[0])/rad)
        segment.set_points((y, x), rad, [angle, -angle])
        segment.draw(axis)
        segment.set_points((y - y_limits[0] + y_limits[1], x), rad, [angle, 2*pi-angle])
        segment.draw(axis)
    elif (y_limits != None) and (y + rad > y_limits[1]):
        segment = toolbox_schematic.CircleSegment()
        segment.set_defaults(alpha=alpha, edgecolor='none', facecolor=col, zorder=zorder)
        angle = numpy.arccos((y_limits[1] - y)/rad)
        segment.set_points((y, x), rad, [angle, 2*pi-angle])
        segment.draw(axis)
        segment.set_points((y + y_limits[0] - y_limits[1], x), rad, [-angle, angle])
        segment.draw(axis)
    else:
        circle = toolbox_schematic.Circle()
        circle.set_defaults(alpha=alpha, edgecolor='none', facecolor=col, zorder=zorder)
        circle.set_points((y, x), rad)
        circle.draw(axis)

def test_water_cost_angle_ic():
    fn_xyz = context.get_fn('test/water_trajectory.xyz')
    system = System.from_file(fn_xyz, ffatypes=['O', 'H', 'H'])
    system.detect_bonds()
    fn_pars = context.get_fn('test/parameters_water.txt')
    parameters = Parameters.from_file(fn_pars)
    del parameters.sections['FIXQ']
    del parameters.sections['DAMPDISP']
    del parameters.sections['EXPREP']

    refpos = np.array([
        [0.0, 0.0, 0.0],
        [0.0, 0.0, 1.1],
        [0.0, 1.1, 0.0],
    ])*angstrom

    rules = [ScaleRule('BENDCHARM', 'PARS', 'H\s*O\s*H', 4)]
    mods = [ParameterModifier(rules)]
    pt = ParameterTransform(parameters, mods)

    simulations = [GeoOptSimulation('only', system)]
    tests = [ICTest(5*deg, refpos, simulations[0], BendGroup(system))]
    assert tests[0].icgroup.cases == [[2, 0, 1]]
    cost = CostFunction(pt, {'all': tests})

    x = np.array([1.0])
    assert abs(cost(x) - np.log(0.5*((np.arccos(2.7892000007e-02) - 1.5707963267948966)/(5*deg))**2)) < 1e-4

    x = np.array([1.1])
    assert abs(cost(x) - np.log(0.5*((np.arccos(1.1*2.7892000007e-02) - 1.5707963267948966)/(5*deg))**2)) < 1e-4

    x = np.array([0.8])
    assert abs(cost(x) - np.log(0.5*((np.arccos(0.8*2.7892000007e-02) - 1.5707963267948966)/(5*deg))**2)) < 1e-4

def make_cluster(n):
    ''' Make n particles in sphere with distribution given by Plummer
    model. '''
    particles = []
    for i in range(n):
        mass   = 1.0 / n         # total mass of system normalised to 1
        radius = 1.0 / N.sqrt( R.random() ** (-2.0/3.0) - 1.0 ) 
        theta = R.uniform(0, 2*N.pi)
        phi   = N.arccos(R.uniform(-1, 1))
        x = radius * N.cos(theta) * N.sin(phi)
        y = radius * N.sin(theta) * N.sin(phi)
        z = radius * N.cos(phi)
        pos = N.array((x, y, z))
        # von Newmann's rejection technique
        a = 0.0
        b = 0.1
        while b > a*a * (1.0 - a*a)**3.5:
            a = R.uniform(0, 1)
            b = R.uniform(0, 0.1)
        velocity = a * N.sqrt(2.0) * (1.0 + radius*radius)**(-0.25)
        theta    = R.uniform(0, 2*N.pi)
        phi      = N.arccos(R.uniform(-1, 1))
        vx = velocity * N.cos(theta) * N.sin(phi)
        vy = velocity * N.sin(theta) * N.sin(phi)
        vz = velocity * N.cos(phi)
        vel = N.array((vx, vy, vz))
        p = Particle(mass, pos, vel)
        particles.append(p)
    return particles

def _get_lw(box):
    p0 = box[0]
    p1 = box[1]
    vec1 = np.array(box[2] - p0)
    vec1 = vec1 / np.linalg.norm(vec1)
    vec2 = np.array(p1 - p0)
    vec2 = vec2 / np.linalg.norm(vec2)
    vec3 = np.array(box[3] - p0)
    vec3 = vec3 / np.linalg.norm(vec3)
    ang1 = np.arccos((vec1).dot(vec2))
    ang2 = np.arccos((vec3).dot(vec2))
    dif1 = 1.5708 - ang1
    dif2 = 1.5708 - ang2
    if dif1 < dif2:
        p2 = box[2]
    else:
        p2 = box[3]
    l, lp = np.linalg.norm(abs(p1 - p0)), p1
    w, wp = np.linalg.norm(abs(p2 - p0)), p2
    if l < w:
        temp = w
        templ = wp
        w = l
        wp = lp
        l = temp
        lp = templ
    direc = (wp - p0) / np.linalg.norm(wp - p0)
    dot = direc.dot(np.array([0, 1]))
    vcost = abs(dot)
    return l, w, vcost

def calculate_couplings_levine(dt: float, w_jk: Matrix,
                               w_kj: Matrix) -> Matrix:
    """
    Compute the non-adiabatic coupling according to:
    `Evaluation of the Time-Derivative Coupling for Accurate Electronic
    State Transition Probabilities from Numerical Simulations`.
    Garrett A. Meek and Benjamin G. Levine.
    dx.doi.org/10.1021/jz5009449 | J. Phys. Chem. Lett. 2014, 5, 2351−2356
    """
    # Orthonormalize the Overlap matrices
    w_jk = np.linalg.qr(w_jk)[0]
    w_kj = np.linalg.qr(w_kj)[0]

    # Diagonal matrix
    w_jj = np.diag(np.diag(w_jk))
    w_kk = np.diag(np.diag(w_kj))

    # remove the values from the diagonal
    np.fill_diagonal(w_jk, 0)
    np.fill_diagonal(w_kj, 0)

    # Components A + B
    acos_w_jj = np.arccos(w_jj)
    asin_w_jk = np.arcsin(w_jk)

    a = acos_w_jj - asin_w_jk
    b = acos_w_jj + asin_w_jk
    A = - np.sin(np.sinc(a))
    B = np.sin(np.sinc(b))

    # Components C + D
    acos_w_kk = np.arccos(w_kk)
    asin_w_kj = np.arcsin(w_kj)

    c = acos_w_kk - asin_w_kj
    d = acos_w_kk + asin_w_kj
    C = np.sin(np.sinc(c))
    D = np.sin(np.sinc(d))

    # Components E
    w_lj = np.sqrt(1 - (w_jj ** 2) - (w_kj ** 2))
    w_lk = -(w_jk * w_jj + w_kk * w_kj) / w_lj
    asin_w_lj = np.arcsin(w_lj)
    asin_w_lk = np.arcsin(w_lk)
    asin_w_lj2 = asin_w_lj ** 2
    asin_w_lk2 = asin_w_lk ** 2

    t1 = w_lj * w_lk * asin_w_lj
    x1 = np.sqrt((1 - w_lj ** 2) * (1 - w_lk ** 2)) - 1
    t2 = x1 * asin_w_lk
    t = t1 + t2
    E_nonzero = 2 * asin_w_lj * t / (asin_w_lj2 - asin_w_lk2)

    # Check whether w_lj is different of zero
    E1 = np.where(np.abs(w_lj) > 1e-8, E_nonzero, np.zeros(A.shape))

    E = np.where(np.isclose(asin_w_lj2, asin_w_lk2), w_lj ** 2, E1)

    cte = 1 / (2 * dt)
    return cte * (np.arccos(w_jj) * (A + B) + np.arcsin(w_kj) * (C + D) + E)