def cc_int(p1, r1, p2, r2):
    """
    Intersect circle (p1,r1) circle (p2,r2)
    where p1 and p2 are 2-vectors and r1 and r2 are scalars
    Returns a list of zero, one or two solution points.
    """
    d = vector.norm(p2-p1)
    if not tol_gt(d, 0):
        return []
    u = ((r1*r1 - r2*r2)/d + d)/2
    if tol_lt(r1*r1, u*u):
        return []
    elif r1*r1 < u*u:
        v = 0.0
    else:
        v = math.sqrt(r1*r1 - u*u)
    s = (p2-p1) * u / d
    if tol_eq(vector.norm(s),0):
        p3a = p1+vector.vector([p2[1]-p1[1],p1[0]-p2[0]])*r1/d
        if tol_eq(r1/d,0):
            return [p3a]
        else:
            p3b = p1+vector.vector([p1[1]-p2[1],p2[0]-p1[0]])*r1/d
            return [p3a,p3b]
    else:
        p3a = p1 + s + vector.vector([s[1], -s[0]]) * v / vector.norm(s) 
        if tol_eq(v / vector.norm(s),0):
            return [p3a]
        else:
            p3b = p1 + s + vector.vector([-s[1], s[0]]) * v / vector.norm(s)
            return [p3a,p3b]

    def __getitem__(self, i):
        """This operation returns either a single value or a subview. Examples:

          >>> m = matrix(array=[[1.,2.,3.],[4.,5.,6.],[7.,8.,9.]])
          >>> m[1]         # row 1
          vector(dtype=float64, shape=(3), data=[4.,5.,6.])
          >>> m[:,1]       # column 1
          vector(dtype=float64, shape=(3), data=[2.,5.,8.])
          >>> m[0,0]       # element (0,0)
          1.0
          >>> m[0:2,0:3:2] # a submatrix
          matrix(dtype=float64, shape=(2,2)
            [[1.0, 3.0],
             [4.0, 6.0]])
        """

        if type(i) != tuple:
            # must be a row accessor
            if isinstance(i, slice):
                return matrix(block=_block.subblock(self.block, (i,slice(0,None))))
            else:
                return vector(block=self.block.row(i))
        assert len(i) == 2
        if isinstance(i[0], slice) and isinstance(i[1], slice):
            return matrix(block=_block.subblock(self.block, i))
        elif isinstance(i[0], slice):
            return vector(block=_block.subblock(self.block.col(i[1]), (i[0],)))
        elif isinstance(i[1], slice):
            return vector(block=_block.subblock(self.block.row(i[0]), (i[1],)))
        else:
            return self.block.get(i[0], i[1])

def make_hcs_3d_scaled (a, b, c):
    """build a 3D homogeneus coordiate system from three points, and derive scale from distance between points"""
     # create orthonormal basis 
    u = normalised(b-a)
    v = normalised(c-a)
    nu = vector.norm(u)
    nv = vector.norm(v)
    if tol_eq(nu,0) and tol_eq(nv,0):
        # all points equal, no rotation
        u = vector.vector([1.0,0.0,0.0])
        v = vector.vector([0.0,1.0,0.0])
    elif tol_eq(nu, 0):
        # determine u perpendicular from v
        u,dummy = perp_3d(v)[0]
    elif tol_eq(nv, 0):
        # determine v perpendicular from u
        dummy,v = perp_3d(u)[0]
    # make the basis vectors orthogonal
    w = vector.cross(u,v)
    v = vector.cross(w,u)
    # scale again
    if not tol_eq(vector.norm(b-a),0.0):
        u = u / vector.norm(b-a)
    if not tol_eq(vector.norm(c-a),0.0):
        v = v / vector.norm(c-a)
    # note: w is not scaled
    # create matix with basis vectors + translation as columns
    hcs = Mat([ 
        [u[0],v[0], w[0], a[0]], 
        [u[1],v[1], w[1], a[1]],
        [u[2],v[2], w[2], a[2]], 
        [0.0, 0.0, 0.0, 1.0]    ])
    return hcs 

 def think(self, others):
     '''
     When the leader is user controlled the leader uses the same seek target logic as without user control.
         The target to be sought to is set in the direction desired by user.
     When the leader is not user controlled the seek target moves using self.seek_offset_speed.
         The position of seek target and self.seek_offset_speed
         is randomized at regular interval or when the leader gets too close to the seek target.
     '''
     if self.user_controlled:
         direction = vector(0, 0)
         if self.user_up: direction.y-= 100
         if self.user_down: direction.y+= 100
         if self.user_left: direction.x-= 100
         if self.user_right: direction.x+= 100
         self.current_seek_target = self.pos + direction
     else:
         distance = self.current_seek_target - self.pos
         if distance.lenght < 10.0 or random.randint(0, 300) == 0:
             self.current_seek_target = self.pos + vector(random.uniform(-50, 50), random.uniform(-50, 50))
             self.randomizeSeekOffsetSpeed()
         else:
             self.seek_offset_speed+= vector(random.uniform(-0.3, 0.3), random.uniform(-0.3, 0.3))
             self.seek_offset_speed.clamp(0, 2.5)
             self.current_seek_target+= self.seek_offset_speed
     #go, seek!
     self.seekTraget(self.current_seek_target, 1.0)
     
     

def make_hcs_3d (a, b, c, righthanded=True):
    """build a 3D homogeneous coordiate system from three points. The origin is point a. The x-axis is
    b-a, the y axis is c-a, or as close as possible after orthogonormalisation."""
    # create orthonormal basis 
    u = normalised(b-a)
    v = normalised(c-a)
    nu = vector.norm(u)
    nv = vector.norm(v)
    if tol_eq(nu,0.0) and tol_eq(nv,0.0):
        # all points equal, no rotation
        u = vector.vector([1.0,0.0,0.0])
        v = vector.vector([0.0,1.0,0.0])
    elif tol_eq(nu, 0.0):
        # determine u perpendicular from v
        u,dummy = perp_3d(v)
    elif tol_eq(nv, 0.0):
        # determine v perpendicular from u
        dummy,v = perp_3d(u)
    # ensure that u and v are different
    if tol_eq(vector.norm(u-v),0.0):
        dummy,v = perp_3d(u)
    # make the basis vectors orthogonal
    w = vector.cross(u,v)
    v = vector.cross(w,u)
    # flip basis if lefthanded desired
    if righthanded==False:
        w = -w
    # create matix with basis vectors + translation as columns
    hcs = Mat([ 
        [u[0],v[0], w[0], a[0]], 
        [u[1],v[1], w[1], a[1]],
        [u[2],v[2], w[2], a[2]], 
        [0.0, 0.0, 0.0, 1.0]    ])
    return hcs 

 def __init__(self, obj, **kwargs):
     super(Physics, self).__init__(obj, **kwargs)
     self.velocity = vector(0,0)
     self.rect = pygame.Rect(0,0,30,30)
     self.corners = (vector(0,0), vector(0,30), vector(30,30), vector(30,0))
     #self.corners = (vector(16,16),)
     self.attached = None
     self.gravity = 1.0

def test_sss_degen():
    p1 = vector.vector([0.0, 0.0, 0.0])
    p2 = vector.vector([2.0, 0.0, 0.0])
    p3 = vector.vector([1.0, 0.0, 0.0])
    r1 = 2.0
    r2 = 2.0
    r3 = math.sqrt(3)
    print sss_int(p1,r1,p2,r2,p3,r3)

 def bounce(self, direction):
     if self['Physics'].velocity.dot(direction) < 0:
         #reflect
         u = direction.normal()
         v = direction
         t = vector(self['Physics'].velocity.dot(u), -self['Physics'].velocity.dot(v))
         self['Physics'].velocity = vector(t.x * u.x + t.y * v.x, t.x * u.y + t.y * v.y) 
         self['Physics'].velocity += direction * self.movement.speed_bonus
     else:
         self['Physics'].velocity += direction * self.movement.speed_bonus

def make_hcs_2d_scaled (a, b):
    """build a 2D homogeneus coordiate system from two points, but scale with distance between input point"""
    u = b-a
    if tol_eq(vector.norm(u), 0.0):     
        u = vector.vector([1.0,0.0])
    #else:
    #    u = u / vector.norm(u)
    v = vector.vector([-u[1], u[0]])
    hcs = Mat([ [u[0],v[0],a[0]] , [u[1],v[1],a[1]] , [0.0, 0.0, 1.0] ] )
    return hcs 

def make_hcs_2d (a, b):
    """build a 2D homogeneus coordiate system from two points"""
    u = b-a
    if tol_eq(vector.norm(u), 0.0):
        u = vector.vector([0.0,0.0])
    else:
        u = u / vector.norm(u)
    v = vector.vector([-u[1], u[0]])
    hcs = Mat([ [u[0],v[0],a[0]] , [u[1],v[1],a[1]] , [0.0, 0.0, 1.0] ] )
    return hcs 

def trilaterate(u, v, r1, r2):
    P1 = vector(u)
    P2 = vector(v)
    ex = (P2 - P1) / abs(P2 - P1)
    d = abs(P2 - P1)
    x = (r1 ** 2 - r2 ** 2 + d ** 2)/(2 * d)
    y1 = math.sqrt(r1 ** 2 - x ** 2)
    y2 = -y1
    p1 = P1 + x * ex + y1
    p2 = P1 + x * ex + y2
    return [p1, p2]

def problem102():
	count = 0
	zero = vector(0,0)
	with open('Problem102.txt','r') as data:
		for l in data.readlines():
			x0,x1,y0,y1,z0,z1 = l.split(',')
			X, Y, Z = vector(x0,x1),vector(y0,y1),vector(z0,z1)
			# Computes the area of each of the possible inner trianle and compares it to the area of the centre
			if area(X,Y,Z) == area(X,Y,zero) + area(X,zero,Z) + area(zero,Y,Z):
				count += 1
	#return all(orthogonalIntersection(v,w, vector(0,0)) for v,w in combinations([X,Y,Z],2))
	return count

def sss_int(p1, r1, p2, r2, p3, r3):
    """Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3 respectively. 
       Returns a list of zero, one or two solution points.
    """
    solutions = []
    # intersect circles in plane
    cp1 = vector.vector([0.0,0.0]) 
    cp2 = vector.vector([vector.norm(p2-p1), 0.0])
    cpxs = cc_int(cp1, r1, cp2, r2)
    if len(cpxs) == 0:
        return []
    # determine normal of plane though p1, p2, p3
    n = vector.cross(p2-p1, p3-p1)
    if not tol_eq(vector.norm(n),0.0):  
        n = n / vector.norm(n)
    else:
        # traingle p1, p2, p3 is degenerate
        # check for 2d solutions
        if len(cpxs) == 0:
            return []
        # project cpxs back to 3d and check radius r3 
        cp4 = cpxs[0]
        u = normalised(p2-p1)
        v,w = perp_3d(u)
        p4 = p1 + cp4[0] * u + cp4[1] * v
        if tol_eq(vector.norm(p4-p3), r3):
            return [p4]
        else:
            return []
    # px, rx, nx is circle 
    px = p1 + (p2-p1) * cpxs[0][0] / vector.norm(p2-p1)
    rx = abs(cpxs[0][1])
    nx = p2-p1
    nx = nx / vector.norm(nx)
    # py is projection of p3 on px,nx
    dy3 = vector.dot(p3-px, nx)
    py = p3 - (nx * dy3)
    if tol_gt(dy3, r3):
        return []
    # ry is radius of circle in py
    if tol_eq(r3,0.0):
        ry = 0.0 
    else:   
        ry = math.sin(math.acos(min(1.0,abs(dy3/r3))))*r3
    # determine intersection of circle px, rx and circle py, ry, projected relative to line py-px 
    cpx = vector.vector([0.0,0.0]) 
    cpy = vector.vector([vector.norm(py-px), 0.0])
    cp4s = cc_int(cpx, rx, cpy, ry)
    for cp4 in cp4s:
        p4 = px + (py-px) * cp4[0] / vector.norm(py-px) + n * cp4[1] 
        solutions.append(p4)  
    return solutions

def mkron(a, *args):
    """Kronecker product of all the arguments"""
    if not isinstance(a, list):
        a = [a]
    a = list(a)  # copy list
    for i in args:
        if isinstance(i, list):
            a.extend(i)
        else:
            a.append(i)

    c = _vector.vector()
    c.d = 0
    c.n = _np.array([], dtype=_np.int32)
    c.r = _np.array([], dtype=_np.int32)
    c.core = []

    for t in a:
        thetensor = t.tt if isinstance(t, _matrix.matrix) else t
        c.d += thetensor.d
        c.n = _np.concatenate((c.n, thetensor.n))
        c.r = _np.concatenate((c.r[:-1], thetensor.r))
        c.core = _np.concatenate((c.core, thetensor.core))

    c.get_ps()
    return c

 def __init__(self, x, y, max_speed = -1, max_force = -1, size = -1, random_position = True):
     '''
     x and y should be the window dimensions when random_position is True.
     '''
     if random_position: self.pos = vector(random.uniform(0, x), random.uniform(0, y))
     else: self.pos = vector(x, y)
     self.speed = vector(0, 0)
     if max_speed > 0: self.max_speed = max_speed
     else: self.randomizeMaxSpeed()
     self.current_force = vector(0, 0)
     if max_force > 0: self.max_force = max_force
     else: self.randomizeMaxForce()
     if size > 0: self.size = size
     else: self.randomizeSize()
     #This is used for drawing
     self.last_seek_target = vector(0, 0)

def improvisedTrilateration(x1, y1, d1, x2,y2,d2, x3,y3,d3):
    p1 = vector.vector()
    p1.append(x1)
    p1.append(y1)

    p2 = vector.vector()
    p2.append(x2)
    p2.append(y2)

    p3 = vector.vector()
    p3.append(x3)
    p3.append(y3)
    a = cc_int(p1, d1, p2, d2)
    b = cc_int(p2, d2, p3, d3)
    c = cc_int(p3, d3, p1, d1)
    return centerFinder(a,b,c)

def eye(n, d=None):
    """ Creates an identity TT-matrix"""
    c = _matrix.matrix()
    c.tt = _vector.vector()
    if d is None:
        n0 = _np.asanyarray(n, dtype=_np.int32)
        c.tt.d = n0.size
    else:
        n0 = _np.asanyarray([n] * d, dtype=_np.int32)
        c.tt.d = d
    c.n = n0.copy()
    c.m = n0.copy()
    c.tt.n = (c.n) * (c.m)
    c.tt.r = _np.ones((c.tt.d + 1,), dtype=_np.int32)
    c.tt.get_ps()
    c.tt.alloc_core()
    for i in xrange(c.tt.d):
        c.tt.core[
            c.tt.ps[i] -
            1:c.tt.ps[
                i +
                1] -
            1] = _np.eye(
            c.n[i]).flatten()
    return c

def random_problem_2D(numpoints, radius=10.0, roundoff=0.0, angleratio=0.5):
    """Generate a random problem with given number of points, a roundoff
       value for the prototype points, a radius for the cloud of prototype points
       and a ratio of angle constraints over distance constraints"""
    group = {}
    problem = GeometricProblem(dimension=2)
    i = 0
    while i < numpoints:
        aname = 'p'+str(i)
        apoint = vector([
            _round(random.uniform(-radius,radius),roundoff),
            _round(random.uniform(-radius,radius),roundoff)
        ])
        unique = True
        for v in group:
            p = group[v]
            if tol_eq(apoint[0],p[0]) and tol_eq(apoint[1],p[1]):
                unique = False
                break
        if unique:
                problem.add_point(aname, apoint)
                group[aname] = apoint
                i = i + 1
    #next
    _constraint_group(problem, group, None, angleratio)
    return problem

def intersectionOfTwoLines(p1,p2,p3,p4):
	''' The intersetion of the two lines spanned by p1,p2 and by p3,p4'''
	x1,y1,x2,y2,x3,y3,x4,y4 = p1.x,p1.y,p2.x,p2.y,p3.x,p3.y,p4.x,p4.y
	denominator = (x1 - x2)*(y3-y4) - (y1-y2)*(x3-x4)
	numeratorx = (x1*y2 - y1*x2)*(x3-x4) - (x1 - x2)*(x3*y4 - y3*x4)
	numeratory = (x1*y2 - y1*x2)*(y3-y4) - (y1 - y2)*(x3*y4 - y3*x4)
	return vector(numeratorx,numeratory)/denominator

def transform_point(point, transform):
    """transform a point"""
    hpoint = Vec(point)
    hpoint.append(1.0)
    hres = transform.mmul(hpoint)
    res = vector.vector(hres[0:-1]) / hres[-1]
    return res