def VectorMath(ExtPts):
    aDx = ExtPts[1].X-ExtPts[0].X
    aDy = ExtPts[1].Y-ExtPts[0].Y

    bDx = ExtPts[-2].X-ExtPts[0].X
    bDy = ExtPts[-2].Y-ExtPts[0].Y

    VectorA=[aDx,aDy]
    VectorB=[bDx,bDy]
    VectorX=[1,0]
    VectorY=[0,1]

    A_Len = np.linalg.norm(VectorA)
    B_Len = np.linalg.norm(VectorB)

    dpAB=dotProduct(VectorA, VectorB)
    AngAB=math.acos(dpAB/A_Len/B_Len)
    cpAB=(A_Len*B_Len)*math.sin(AngAB)

    dpAX=dotProduct(VectorA, VectorX)
    AngAx=math.acos(dpAX/A_Len/1.0)*math.copysign(1,aDy)

    dpBX=dotProduct(VectorB, VectorX)
    AngBx=math.acos(dpBX/B_Len/1.0)*math.copysign(1,bDy)

    eParts=[A_Len,B_Len, AngAB, AngAx, AngBx,cpAB]

    return(eParts)

def my_solid_angle(center, coords):
    """
    Helper method to calculate the solid angle of a set of coords from the
    center.

    Args:
        center:
            Center to measure solid angle from.
        coords:
            List of coords to determine solid angle.

    Returns:
        The solid angle.
    """
    o = np.array(center)
    r = [np.array(c) - o for c in coords]
    r.append(r[0])
    n = [np.cross(r[i + 1], r[i]) for i in range(len(r) - 1)]
    n.append(np.cross(r[1], r[0]))
    phi = 0.0
    for i in range(len(n) - 1):
        try:
            value = math.acos(-np.dot(n[i], n[i + 1]) / (np.linalg.norm(n[i]) * np.linalg.norm(n[i + 1])))
        except ValueError:
            mycos = -np.dot(n[i], n[i + 1]) / (np.linalg.norm(n[i]) * np.linalg.norm(n[i + 1]))
            if 0.999999999999 < mycos < 1.000000000001:
                value = math.acos(1.0)
            elif -0.999999999999 > mycos > -1.000000000001:
                value = math.acos(-1.0)
            else:
                raise SolidAngleError(mycos)
        phi += value
    return phi + (3 - len(r)) * math.pi

def ikCalc(bodyHeight, footDistance):  # ik Calc
    # bodyHeight = 35
    # footDistance = 80
    femurLength = 70
    tibiaLength = 111
    hypo = math.sqrt(((math.pow(footDistance, 2)) + (math.pow(bodyHeight, 2))))
    alpha = (
        (
            math.acos(
                (math.pow(femurLength, 2) + math.pow(hypo, 2) - math.pow(tibiaLength, 2)) / (2 * femurLength * hypo)
            )
        )
        * 180
        / math.pi
    )
    gamma = (math.atan2(bodyHeight, footDistance)) * 180 / math.pi
    beta = 90 - alpha + gamma
    beta = int(beta * 100) / 100.0
    delta = (
        (
            math.acos(
                (math.pow(femurLength, 2) + math.pow(tibiaLength, 2) - math.pow(hypo, 2))
                / (2 * femurLength * tibiaLength)
            )
        )
        * 180
        / math.pi
    )
    epsilon = 180 - delta
    epsilon = int(epsilon * 100) / 100.0
    print "Body Height: " + str(bodyHeight) + " Foot Distance: " + str(footDistance) + " Beta: " + str(
        beta
    ) + " Epsilon: " + str(epsilon)
    return (beta, epsilon)

def _compute_disk_overlap(d, r1, r2):
    """
    Compute surface overlap between two disks of radii ``r1`` and ``r2``,
    with centers separated by a distance ``d``.

    Parameters
    ----------
    d : float
        Distance between centers.
    r1 : float
        Radius of the first disk.
    r2 : float
        Radius of the second disk.

    Returns
    -------
    vol: float
        Volume of the overlap between the two disks.
    """

    ratio1 = (d ** 2 + r1 ** 2 - r2 ** 2) / (2 * d * r1)
    ratio1 = np.clip(ratio1, -1, 1)
    acos1 = math.acos(ratio1)

    ratio2 = (d ** 2 + r2 ** 2 - r1 ** 2) / (2 * d * r2)
    ratio2 = np.clip(ratio2, -1, 1)
    acos2 = math.acos(ratio2)

    a = -d + r2 + r1
    b = d - r2 + r1
    c = d + r2 - r1
    d = d + r2 + r1
    area = (r1 ** 2 * acos1 + r2 ** 2 * acos2 -
            0.5 * sqrt(abs(a * b * c * d)))
    return area / (math.pi * (min(r1, r2) ** 2))

 def __init__(self, from_obj, to_obj):
     """Create an affine transformation (rotation+translation) that
     takes from_obj to to_obj in some sense.
     """
     # self.dr is always defined
     self.q = None # rotation quaternion, if applicable
     if isinstance(from_obj,Point) and isinstance(to_obj,Point):
         self.dr = to_obj.r - from_obj.r
     elif isinstance(from_obj,Point) and isinstance(to_obj,Line):
         # move point to closest point on line
         p = from_obj.projected_on(to_obj)
         self.dr = p.r - from_obj.r
     elif isinstance(from_obj,Point) and isinstance(to_obj,Plane):
         # move point to nearest point in plane
         p = from_obj.projected_on(to_obj)
         self.dr = p.r - from_obj.r
     elif isinstance(from_obj,Line) and isinstance(to_obj,Line):
         if dot(from_obj.t,to_obj.t) < 1 - 1e-14:
             self.q = qrotor(cross(from_obj.t,to_obj.t),
                             math.acos(dot(from_obj.t,to_obj.t)))
             self.dr = orthogonalized_to(to_obj.r - qrotate(self.q,from_obj.r),to_obj.t)
         else:
             self.dr = orthogonalized_to(to_obj.r - from_obj.r,to_obj.t)
     elif isinstance(from_obj,Line) and isinstance(to_obj,Plane):
         lp = from_obj.projected_on(to_obj)
         return Movement.__init__(self,from_obj,lp)
     elif isinstance(from_obj,Plane) and isinstance(to_obj,Plane):
         if dot(from_obj.n,to_obj.n) < 1 - 1e-14:
             self.q = qrotor(cross(from_obj.n,to_obj.n),
                             math.acos(dot(from_obj.n,to_obj.n)))
             self.dr = to_obj.r - qrotate(self.q,from_obj.r)
         else:
             self.dr = orthogonalized_to(to_obj.r - from_obj.r,to_obj.n)

def transform(x,y,z):
    initial_angle=math.acos(L/(4*l))#
    bed_angle=math.asin((z-square_z)/l)#
    leg_offset=l*math.cos(bed_angle)
    yprime=y+y_offset-leg_offset
    xprime=x+L/2
    topz=((d/2-y)/d)*front_z+(1-(d/2-y)/d)*back_z
    bed_tilt=math.atan2(-back_z+front_z,d)
    yprime-=z*math.sin(bed_tilt)
    zprime=topz-z*math.cos(bed_tilt)
    left_leg=math.sqrt(xprime*xprime+yprime*yprime)
    right_leg=math.sqrt((L-xprime)*(L-xprime)+yprime*yprime)
    left_elbow=math.acos((left_leg*left_leg-2*l*l)/(-2*l*l))
    right_elbow=math.acos((right_leg*right_leg-2*l*l)/(-2*l*l))
    left_small_angle=(math.pi-left_elbow)/2
    right_small_angle=(math.pi-right_elbow)/2
    left_virtual=math.atan(-yprime/xprime)
    right_virtual=math.atan(-yprime/(L-xprime))
    left_drive=left_small_angle+left_virtual-initial_angle
    right_drive=right_small_angle+right_virtual-initial_angle
    left_stepper=-left_drive+(math.pi-left_elbow)*mechanical_advantage
    right_stepper=-right_drive+(math.pi-right_elbow)*mechanical_advantage
    #print "left_angle: "+str(left_drive)+" left_elbow: "+str(math.pi-left_elbow)
    #print "right_angle: "+str(left_drive)+" right_elbow: "+str(math.pi-right_elbow)
    return left_stepper*200/math.pi,right_stepper*200/math.pi,zprime

def matrix2abcabg(lattice):
    la,lb,lc = lattice
    a,b,c = length(la),length(lb),length(lc)
    gamma = math.acos(dot(la,lb)/a/b)*180/math.pi
    beta =  math.acos(dot(la,lc)/a/c)*180/math.pi
    alpha = math.acos(dot(lb,lc)/b/c)*180/math.pi
    return a,b,c,alpha,beta,gamma

def rotation_by2vec(orient,orient2):
    initial_orient=[0,0,1]
    initial_orient2=[1,0,0]
    initial_orient=vec_normalize(initial_orient)
    initial_orient2=vec_normalize(initial_orient2)
    orient= vec_normalize(orient)
    rotation_angle= math.acos(dot_product(initial_orient,orient))
    rotation_axis=cross_product(initial_orient,orient)
    if vec_distance(rotation_axis,[0,0,0]) == 0:
	# if initial_orient and orient are parellel then we  can use any vector perpendicular to this line as the rotation axis 
	rotation_axis=perp_vec(orient)
    rotation_axis= vec_normalize(rotation_axis)
    rotation_matr1= rotation_matrix(rotation_axis,rotation_angle)
    initial_orient2= matrix_vec_multiply(rotation_matr1,initial_orient2)
    orient2= vec_scale(orient2,1 - dot_product(orient2,orient))
    orient2= vec_normalize(orient2)
    rotation_angle= math.acos(dot_product(initial_orient2,orient2))
    rotation_axis= cross_product(initial_orient2,orient2)
    if vec_distance(rotation_axis,[0,0,0]) == 0:
	# if initial_orient and orient are parellel then we  can use any vector perpendicular to this line as the rotation axis
	rotation_axis=perp_vec(orient2)
    rotation_axis= vec_normalize(rotation_axis)
    rotation_matr2= rotation_matrix(rotation_axis,rotation_angle)
    rotation_matr= matrix_multiply(rotation_matr2,rotation_matr1)
    
    return rotation_matr

	def aggregateAngularForce(points1, points2):
		unitVecs = []
		com1 = PointAligner.centerOfMass(points1)
		com2 = PointAligner.centerOfMass(points2)

		#calculate unit vectors in each theta angle
		for (x1,y1) in points1:
			for (x2,y2) in points2:

				if (x1==x2 and y1==y2):
					continue

				r = PointAligner.distance(x1,y1,x2,y2)
				x3 = x1-x2
				y3 = y1-y2
				tanVec = [com2[1]-y2,-(com2[0]-x2)]

				if(tanVec[0]==0 and tanVec[1]==0):
					continue

				theta = math.acos( (x3*tanVec[0]-y3*tanVec[1]) / (PointAligner.length(x3,y3) * 
								PointAligner.length(tanVec[0],tanVec[1])))
				unitVecs.append( [[math.cos(theta), math.sin(theta)],r] )

		#average the unit vectors
		aggregate = [0,0]
		totalWeight = 0
		for [[x,y],r] in unitVecs:
			aggregate[0] += x*r
			aggregate[1] += y*r
			totalWeight += r
		print "agg = " + `aggregate`
		if aggregate==[0,0]:
			return 0;
		return math.acos( (1*aggregate[0]) / ( PointAligner.length(aggregate[0],aggregate[1])))

    def get_sight_direction(self):
        dx, dy, dz = self.get_sight_vector()
        dz = -dz

        angle = 0
        if dz == 0:
            if dx > 0:
                angle = 0
            elif dx < 0:
                angle = pi
        elif dz > 0:
            angle = acos(dx / sqrt(dx * dx + dz * dz))
        else:
            angle = -acos(dx / sqrt(dx * dx + dz * dz))

        if angle < 0: angle += 2 * pi

        angle *= 180 / pi

        if 0 < angle <= 45 or 315 < angle <= 360:
            direction = G.EAST
        elif 45 < angle <= 135:
            direction = G.NORTH
        elif 135 < angle <= 225:
            direction = G.WEST
        elif 225 < angle <= 315:
            direction = G.SOUTH

        return (dx, dy, dz), direction, angle

def calculate_reciprocal_lattice(lattice):
    """calculate reciprocal lattice

    :param lattice:
    :return:
    """
    # check
    assert isinstance(lattice, Lattice)

    # calculate reciprocal lattice
    lattice_star = Lattice(1, 1, 1, 0, 0, 0)

    # calculate volume
    volume = 2 * lattice.get_a() * lattice.get_b() * lattice.get_c() * numpy.sqrt(
        m_sin((lattice.get_alpha() + lattice.get_beta() + lattice.get_gamma()) / 2) *
        m_sin((-lattice.get_alpha() + lattice.get_beta() + lattice.get_gamma()) / 2) *
        m_sin((lattice.get_alpha() - lattice.get_beta() + lattice.get_gamma()) / 2) *
        m_sin((lattice.get_alpha() + lattice.get_beta() - lattice.get_gamma()) / 2))
    #  v_start = (2 * numpy.pi) ** 3. / volume

    # calculate a*, b*, c*
    lattice_star.set_a(2 * numpy.pi * lattice.get_b() * lattice.get_c() * m_sin(lattice.get_alpha()) / volume)
    lattice_star.set_b(2 * numpy.pi * lattice.get_a() * lattice.get_c() * m_sin(lattice.get_beta()) / volume)
    lattice_star.set_c(2 * numpy.pi * lattice.get_b() * lattice.get_a() * m_sin(lattice.get_gamma()) / volume)

    lattice_star.set_alpha(math.acos((m_cos(lattice.get_beta()) * m_cos(lattice.get_gamma()) - m_cos(lattice.get_alpha())) /
                                     (m_sin(lattice.get_beta()) * m_sin(lattice.get_gamma()))) * 180. / numpy.pi)
    lattice_star.set_beta(math.acos((m_cos(lattice.get_alpha()) * m_cos(lattice.get_gamma()) - m_cos(lattice.get_beta())) /
                                    (m_sin(lattice.get_alpha()) * m_sin(lattice.get_gamma()))) * 180. / numpy.pi)
    lattice_star.set_gamma(math.acos((m_cos(lattice.get_alpha()) * m_cos(lattice.get_beta()) - m_cos(lattice.get_gamma())) /
                                     (m_sin(lattice.get_alpha()) * m_sin(lattice.get_beta()))) * 180. / numpy.pi)

    return lattice_star

def rgb2hsi(_red: float, _green: float, _blue: float) -> tuple:
    """RGB -> HSI"""
    _red = round(_red * 255, 6)
    _green = round(_green * 255, 6)
    _blue = round(_blue * 255, 6)
    if isinstance(_red, str) or _red < 0:
        _red = 0
    if isinstance(_green, str) or _green < 0:
        _green = 0
    if isinstance(_blue, str) or _green < 0:
        _blue = 0
    _rgbmin = min(_red, _green, _blue)
    _rgbplus = _red + _green + _blue
    _red2 = _red * _red
    _green2 = _green * _green
    _blue2 = _blue * _blue
    _var0 = (_red * _green) - (_red * _blue) - (_green * _blue)
    _squareroot = round(sqrt(_red2 + _green2 + _blue2 - _var0), 6)
    _var1 = _red - (_green * 0.5) - (_blue * 0.5)
    _var2 = _var1 / _squareroot
    _radconv = 57.2958279088
    _intensity = (_red + _green + _blue) * 0.33333333
    if _rgbplus == 765:
        _sat = 0
        _hue = 0
        return _hue, _sat, round(_intensity, 6)
    if _intensity > 0:
        _sat = 1 - (_rgbmin / _intensity)
    elif _intensity == 0:
        _sat = 0
    if _green >= _blue:
        _hue = _radconv * acos(_var2)
    elif _blue > _green:
        _hue = 360 - (_radconv * acos(_var2))
    return round(_hue, 6), round(_sat, 6), round(_intensity, 6)

def GetVecAngle(x, y):
    r_xy = math.sqrt(x * x + y * y)
    if (y >= 0):
        VecAngle = math.acos(x / r_xy)
    else:
        VecAngle = 2 * math.pi - math.acos(x / r_xy)
    return VecAngle

def geo_centroid(lat_array, lon_array, load_array):
    geo_data = pd.DataFrame(columns=['lat', 'lon', 'load'])
    geo_data.lat = lat_array
    geo_data.lon = lon_array
    geo_data.load = load_array
    R = 3963.1676 # miles
    geo_data['xi'] = [math.radians(90-x) for x in geo_data.lat]
    geo_data['theta'] = [math.radians(x) for x in geo_data.lon]
    geo_data['X'] = [R*math.sin(x)*math.cos(t) for (x,t) in zip(geo_data.xi, geo_data.theta)]
    geo_data['Y'] = [R*math.sin(x)*math.sin(t) for (x,t) in zip(geo_data.xi, geo_data.theta)]
    geo_data['Z'] = [R*math.cos(x) for x in geo_data.xi]
    X_centroid = np.average(geo_data.X, weights=geo_data.load)
    Y_centroid = np.average(geo_data.Y, weights=geo_data.load)
    Z_centroid = np.average(geo_data.Z, weights=geo_data.load)
    x_var = np.average((geo_data.X-X_centroid)**2, weights=geo_data.load)
    y_var = np.average((geo_data.Y-Y_centroid)**2, weights=geo_data.load)
    z_var = np.average((geo_data.Z-Z_centroid)**2, weights=geo_data.load)
    porsigma = math.sqrt(x_var + y_var + z_var)
    L = math.sqrt(X_centroid*X_centroid + Y_centroid*Y_centroid + Z_centroid*Z_centroid)
    por = L/R
    xi = math.acos(Z_centroid/L)
    theta = math.acos(X_centroid/L/math.sin(xi)) if Y_centroid/math.sin(xi) > 0 else math.acos(X_centroid/L/math.sin(xi))*(-1)
    lat_centroid = 90 - xi/math.radians(1)
    lon_centroid = theta/math.radians(1)
    return [round(lat_centroid,3), round(lon_centroid,3), round(por,4), round(porsigma,2)]

def get_sector(prev_v, spike_v, next_v, threshold=0.0):
    '''
    Returns 2 vectors that define the sector rays.
    The vectors have unit lengths
    The vector pair is right-hand
    '''
    vec1 = (spike_v - prev_v).normalized()
    vec2 = (spike_v - next_v).normalized()

    cosine = Geom2.cos_angle(vec1, vec2)
    sine = Geom2.sin_angle(vec1, vec2)

    sector_angle = 2 * math.pi - math.acos(cosine) if sine < 0 else math.acos(cosine)

    # degrees to radian
    clearance = math.pi * threshold / 180.0
    sector_angle_plus = sector_angle + clearance

    
    # limit sector opening to 180 degrees:
    sector_angle_plus = min(sector_angle_plus, math.pi)

    clearance = .5 * (sector_angle_plus - sector_angle)

    cosine = math.cos(clearance)
    sine = math.sin(clearance)

    # rotate sector vectors to increase angle for clearance
    return (
        Geom2.mul_mtx_2x2((cosine, sine, -sine, cosine), vec1),
        Geom2.mul_mtx_2x2((cosine, -sine, sine, cosine), vec2)
    )

 def getAngle(self,cc):
         r = ( (cc[2] - cc[0]) ** 2 + (cc[3] - cc[1] ) ** 2 ) ** 0.5
         print('current R is:%8.4f' % r)
         print (cc)
         x = cc[2] - cc[0]
         y = cc[3] - cc[1]
         print(x,y)
         sina = y / r
         cosa = x / r
         starta = float()
         if y > 0 and x > 0:
             starta=math.asin ( sina ) ## 0 ~ PI/2
             print ("q1: %f" % starta)
         else:
             if  y > 0 and x <=0:
                 starta = math.acos(cosa) ## PI/2 ~ PI
                 print ("q2: %f" % starta)
             else:
                 if y <=0 and x <=0:
                     starta = math.pi*2 - math.acos(cosa)
                     print("q3: %f" % starta)
                 else:
                     starta = math.pi * 2 + math.asin(sina)
                     print("q4: %f" % starta)
         print("The angle of the hand is: %f" % starta)
         print('The coords of the hand are (after calculated from the Angle: is %s' % cc)
         return (starta)

def get_desired_drot(goal,rot,drot):
	slow_down_dist = 90*DEGTORAD #radians
	max_speed = 0.0146221 #radians per timestep

	dot = Vector(0,1,0).dot(goal.normalise());
	axis = goal.cross(Vector(0,1,0)).normalise()
	alpha = acos(dot)
	if axis == None and dot == 1: #parallel
		desired_rot = Quaternion(1,0,0,0)
	elif axis == None and dot == -1: #opposite
		desired_rot = Quaternion(0,1,0,0)
	else:
		desired_rot = Quaternion(cos(alpha/2),axis.x*sin(alpha/2),axis.y*sin(alpha/2),axis.z*sin(alpha/2))

	to_desired = (desired_rot * rot.inverse()).normalise()
	dist = acos(to_desired.w)*2;
	if dist > 0:
		axis = Vector(to_desired.x,to_desired.y,to_desired.z).normalise()
		if dist < slow_down_dist:
			theta = max_speed*dist/slow_down_dist
		else:
			theta = max_speed
		return Quaternion(cos(theta/2),axis.x*sin(theta/2),axis.y*sin(theta/2),axis.z*sin(theta/2))
	else:
		return Quaternion(1,0,0,0)

def solve_2R_inverse_kinematics(x,y,L1=1,L2=1):
    """For a 2R arm centered at the origin, solves for the joint angles
    (q1,q2) that places the end effector at (x,y).

    The result is a list of up to 2 solutions, e.g. [(q1,q2),(q1',q2')].
    """
    D = vectorops.norm((x,y))
    thetades = math.atan2(y,x)
    if D == 0:
        raise ValueError("(x,y) at origin, infinite # of solutions")
    c2 = (D**2-L1**2-L2**2)/(2.0*L1*L2)
    q2s = []
    if c2 < -1:
        print "solve_2R_inverse_kinematics: (x,y) inside inner circle"
        return []
    elif c2 > 1:
        print "solve_2R_inverse_kinematics: (x,y) out of reach"
        return []
    else:
        if c2 == 1:
            q2s = [math.acos(c2)]
        else:
            q2s = [math.acos(c2),-math.acos(c2)]
    res = []
    for q2 in q2s:
        thetaactual = math.atan2(math.sin(q2),L1+L2*math.cos(q2))
        q1 = thetades - thetaactual
        res.append((q1,q2))
    return res

 def setSSS(self, lenp3, lenp1, lenp2):
     self.lenp3 = lenp3
     self.lenp1 = lenp1
     self.lenp2 = lenp2
     self.ap1 = math.acos(((self.lenp2 * self.lenp2 + self.lenp3 * self.lenp3) - self.lenp1 * self.lenp1) / (2* self.lenp2 * self.lenp3))
     self.ap2 = math.acos(((self.lenp1 * self.lenp1 + self.lenp3 * self.lenp3) - self.lenp2 * self.lenp2) / (2* self.lenp1 * self.lenp3))
     self.ap3 = math.acos(((self.lenp1 * self.lenp1 + self.lenp2 * self.lenp2) - self.lenp3 * self.lenp3) / (2* self.lenp1 * self.lenp2))

    def tf_person_2_contam_grid(self, x, y, theta, orientation, frame_id):
        # transform from ellipse frame to map frame and get ellipse data
        # FORMULA ASSUMES LASER AND MAP ARE ROTATED ONLY AROUND Z AXIS
        try:
            self.tf_listener.waitForTransform(self.ogrid_frame_id, frame_id, rospy.Time(0), rospy.Duration(0.5))
            (trans, rot) = self.tf_listener.lookupTransform(self.ogrid_frame_id, "/" + frame_id, rospy.Time(0))

            angle = acos(rot[3]) * 2
            center = (x * cos(angle) - y * sin(angle) + trans[0], x * sin(angle) + y * cos(angle) + trans[1])
            q = orientation
            w = q.w * rot[3] - q.x * rot[0] - q.y * rot[1] - q.z * rot[2]
            theta = acos(w) * 2

            """
            x = ellipse.pose.position.x
            y = ellipse.pose.position.y
            angle = acos(rot[3]) * 2

            center = (x*cos(angle) - y*sin(angle) + trans[0], x*sin(angle) + y*cos(angle) + trans[1])
            (a, b) = (ellipse.scale.x/2, ellipse.scale.y/2)
            
            q = ellipse.pose.orientation
            w = (q.w*rot[3] - q.x*rot[0] - q.y*rot[1] - q.z*rot[2])
            theta = acos(w)*2
            """

            return center, theta

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            print "no tf found"
            return None