def filter_stabilized():  # Analog zu filter_reference()
    global sens_stab
    offset = [0.0, 0.0]
    sens_stab = [0.0, 0.0]
    out = [0.0, 0.0]
    iterations = 0
    k = 0.025
    time.sleep(1)
    while True:
        out[0] = (
            offset[0]
            + 0.99 * (out[0] + stabilized_gyro_omega[0] * DT - offset[0])
            + 0.01
            * (360 / (2 * math.pi))
            * (math.atan2(stabilized_accelerometer_acc["y"], stabilized_accelerometer_acc["z"]) + math.pi)
        )
        out[1] = (
            offset[1]
            + 0.99 * (out[1] + stabilized_gyro_omega[1] * DT - offset[1])
            + 0.01
            * (360 / (2 * math.pi))
            * (math.atan2(stabilized_accelerometer_acc["z"], stabilized_accelerometer_acc["x"]) + math.pi)
        )
        iterations += 1
        time.sleep(k * DT)
        if iterations == 30 * (1 / DT):
            offset[0] = 0.0 - out[0]
            offset[1] = 0.0 - out[1]
            out[0] = 0
            out[1] = 0
            print "Sensor 2 calibrated"
            k = 1
        sens_stab[0] = int(out[0])
        sens_stab[1] = int(out[1])

def isRectangle(pts, seuil=0.05):
    if len(pts) != 4:
        raise ValueError('Input must be 4 points')
    coords = pts.ravel().reshape(-1, 2)
    cx, cy = np.mean([coord[0] for coord in coords]), np.mean(
        [coord[1] for coord in coords])
    dist = [distance((cx, cy), coord) for coord in coords]
    res = 0
    # print coords
    for i in xrange(1, 4):
        res += abs(dist[0] - dist[i])
    bias = res / distance(coords[1], coords[2])
    logging.info('Regtangle bias: %.3f', res)
    logging.info('Ratio bias: %.3f', bias)
    if bias < seuil:
        line1 = coords[3] - coords[0]
        line2 = coords[2] - coords[1]
        mean_radian = - \
            (math.atan2(line1[1], line1[0]) +
             math.atan2(line2[1], line2[0])) / 2
        inclination = math.degrees(mean_radian)  # / np.pi * 90
        logging.info('Document rotation: %.3f degree', inclination)
        return True, mean_radian
    else:
        return False, None

def matrix_to_euler(rotmat):
    '''Inverse of euler_to_matrix().'''
    if not isinstance(rotmat, np.matrixlib.defmatrix.matrix):
        # As this calculation relies on np.matrix algebra - convert array to
        # matrix
        rotmat = np.matrix(rotmat)

    def cvec(x, y, z):
        return np.matrix([[x, y, z]]).T
    ex = cvec(1., 0., 0.)
    ez = cvec(0., 0., 1.)
    exs = rotmat.T * ex
    ezs = rotmat.T * ez
    enodes = np.cross(ez.T, ezs.T).T
    if np.linalg.norm(enodes) < 1e-10:
        enodes = exs
    enodess = rotmat * enodes
    cos_alpha = float((ez.T*ezs))
    if cos_alpha > 1.:
        cos_alpha = 1.
    if cos_alpha < -1.:
        cos_alpha = -1.
    alpha = acos(cos_alpha)
    beta = np.mod(atan2(enodes[1, 0], enodes[0, 0]), pi * 2.)
    gamma = np.mod(-atan2(enodess[1, 0], enodess[0, 0]), pi*2.)
    return unique_euler(alpha, beta, gamma)

 def update_searchlight_from_orientation_(self):
   qw, qx, qy, qz = self.move.get_orientation()
   # Convert quaternion to euler-angle.
   # Formulas from:
   # http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
   # but note that all values are negated.
   azimuth = 0
   elevation = 0
   test = qx * qy + qz * qw
   roll = math.asin(2 * test)
   if test > 0.499:
     azimuth = -2 * math.atan2(qx, qw)
     elevation = math.pi / 2
   elif test < -0.499:
     azimuth = 2 * math.atan2(qx, qw)
     elevation = 0
   else:
     azimuth = -math.atan2(2 * qy * qw - 2 * qx * qz,
                           1 - 2 * qy * qy - 2 * qz * qz)
     elevation = math.atan2(2 * qx * qw - 2 * qy * qz ,
                            1 - 2 * qx * qx - 2 * qz * qz)
   # print 'az %.2f el %.2f roll %.2f' % (
   #     azimuth * 57.2957795, elevation * 57.2957795, roll * 57.2957795)
   min_elevation = -1
   for searchlight in self.searchlights:
     min_elevation = max(min_elevation, searchlight.config.elevation_lower_bound)
   min_elevation = clamp_and_scale(min_elevation, -1, 1, 0, 90 * DEGREES_TO_RADIANS)
   if elevation < min_elevation:
     elevation = min_elevation
   self.reactor.callFromThread(self.target_angle, azimuth, elevation)

def pointObjectToTarget(obj, targetLoc):
    dx = targetLoc.x - obj.location.x;
    dy = targetLoc.y - obj.location.y;
    dz = targetLoc.z - obj.location.z;
    xRad = math.atan2(dz, math.sqrt(dy**2 + dx**2)) + math.pi/2;
    zRad = math.atan2(dy, dx) - math.pi/2;
    obj.rotation_euler = mathutils.Euler((xRad, 0, zRad), 'XYZ');

def draw_car(x,y,th,canv):    
    r = math.sqrt(math.pow(ROBOT_LENGTH,2) + math.pow(ROBOT_WIDTH,2))/2    
    x = x + 300.0/cm_to_pixels
    y = y + 300.0/cm_to_pixels
    
    # top left
    phi = th + math.pi/2+ math.atan2(ROBOT_LENGTH,ROBOT_WIDTH)
    topleft  = (x + r*math.cos(phi),y+r*math.sin(phi))
    
    #top right
    phi = th + math.atan2(ROBOT_WIDTH,ROBOT_LENGTH) 
    topright = (x + r*math.cos(phi),y+r*math.sin(phi))

    # bottom left
    phi = th + math.pi + math.atan2(ROBOT_WIDTH,ROBOT_LENGTH)
    bottomleft =  (x + r*math.cos(phi),y+r*math.sin(phi))
    
    # bottom right
    phi = th + 3*math.pi/2 + math.atan2(ROBOT_LENGTH,ROBOT_WIDTH)
    bottomright =  (x + r*math.cos(phi),y+r*math.sin(phi))
    
    canv.create_polygon(topleft[0]*cm_to_pixels,600 - topleft[1]*cm_to_pixels,
                        bottomleft[0]*cm_to_pixels,600 - bottomleft[1]*cm_to_pixels,
                        bottomright[0]*cm_to_pixels,600 - bottomright[1]*cm_to_pixels,
                        topright[0]*cm_to_pixels,600 - topright[1]*cm_to_pixels,
                        width = 1, outline = 'blue',fill = '')
    x1 = x*cm_to_pixels
    y1 = y*cm_to_pixels
    canv.create_oval(x1-1,600-(y1-1),x1+1,600-(y1+1),outline = 'green',fill = 'green')

    def getValues(self):        
        accx = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_X_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_X_LSB))
        accy = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_Y_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_Y_LSB))
        accz = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_Z_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_Z_LSB))

        gyrox = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_X_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_X_LSB))
        gyroy = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_Y_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_Y_LSB))
        gyroz = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_Z_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_Z_LSB))

        rate_gyrox = gyrox*self.GYRO_ADD
        rate_gyroy = gyroy*self.GYRO_ADD
        rate_gyroz = gyroz*self.GYRO_ADD

        if (not self.FIRST):
            self.FIRST = True
            self.gyroXangle = rate_gyrox*self.DT
            self.gyroYangle = rate_gyroy*self.DT
            self.gyroZangle = rate_gyroz*self.DT
        else:
            self.gyroXangle += rate_gyrox*self.DT
            self.gyroYangle += rate_gyroy*self.DT
            self.gyroZangle += rate_gyroz*self.DT

        roll = int(round(math.degrees(math.atan2(accx, accz))))
        pitch = int(round(math.degrees(math.atan2(accy, accz))))

        print "Przechylenie: ", int(round(roll,0)), " Pochylenie: ", int(round(pitch,0))

        self.FILTR_X = self.MULTIPLY*(roll)+(1-self.MULTIPLY)*self.gyroXangle
        self.FILTR_Y = self.MULTIPLY*(pitch)+(1-self.MULTIPLY)*self.gyroYangle

        print "Filtr przechylenie: ", int(round(self.FILTR_X,0)), " Filtr pochylenie: ", int(round(self.FILTR_Y,0))

        return str(roll)+';'+str(pitch)

    def getArcParameters(self, arc, version):
        xs = self.setUnit(arc['pos'][0], version)
        ys = self.setUnit(arc['pos'][1], version)
        
        x1 = self.setUnit(arc['ofsa'][0], version)
        y1 = self.setUnit(arc['ofsa'][1], version)
        
        x2 = self.setUnit(arc['ofsb'][0], version)
        y2 = self.setUnit(arc['ofsb'][1], version)
        
        angle = float("%.1f" % ((atan2(y2, x2) - atan2(y1, x1)) * 180 / 3.14))
        #
        x1 = self.setUnit(arc['ofsa'][0], version) + xs
        y1 = self.setUnit(arc['ofsa'][1], version) + ys
        
        [x2R, y2R] = self.obrocPunkt2([x1, y1], [xs, ys], angle)
        
        #FreeCAD.Console.PrintWarning(u"angle = {0}\n".format(angle))
        #FreeCAD.Console.PrintWarning(u"xs = {0}\n".format(xs))
        #FreeCAD.Console.PrintWarning(u"ys = {0}\n".format(ys))
        #FreeCAD.Console.PrintWarning(u"x1 = {0}\n".format(x1))
        #FreeCAD.Console.PrintWarning(u"y1 = {0}\n".format(y1))
        #FreeCAD.Console.PrintWarning(u"x2R = {0}\n".format(x2R))
        #FreeCAD.Console.PrintWarning(u"y2R = {0}\n\n".format(y2R))
        
        x2R = float("%.1f" % x2R)
        y2R = float("%.1f" % y2R)
        
        if angle < 0:
            angle = angle - 360
            
        width = self.setUnit(arc['width'], version)

        return [x1, y1 * -1, x2R, y2R * -1, angle, width]

def on_mouse_press(x, y, button, modifiers):
    #print player1.x, player1.y
    #for n in range(0, player1.sides):
        #print n, " ::: ", player1.vertices[2 * n + 2], player1.vertices[2 * n + 3], " : ", player1.colors[4*n + 4:4*n+8]
    if button == mouse.MIDDLE:
        player1.drawTarget = True
    if button == mouse.LEFT and player1.sides > 3:
        a1 = math.atan2(float(y - player1.y), float(x - player1.x))
        side = 0
        closest = 3.1415
        a3 = 100
        for n in range(1, player1.sides + 1):
            vx = player1.vertices[2*n]
            vy = player1.vertices[2*n + 1]
            a2 = math.atan2((vy - player1.y), (vx - player1.x))
            if abs(a2 - a1) < closest:
                side = n
                closest = abs(a2 - a1)
                a3 = a2

        vtx, clr = player1.removeSide(side)
        #player1.x + math.cos(angle) * (player1.radius + 4), player1.y + math.sin(angle) * (player1.radius + 4)

        b = bullet(angle = a1, vtx=vtx, color = clr)
        game_objects.append(b)

	def pid(self, data):
		#Function that does a z control over the heading for the ground vehicle
		#Uses z from the ar tag and tries to reach it
		
		#Calculate heading angle to goal
		u_x = data.x - self.x
		u_y = data.y - self.y 
		theta_g = math.atan2(u_y, u_x)
		
		#Calculate error in heading
		e_k = theta_g - self.theta
		e_k = math.atan2(math.sin(e_k), math.cos(e_k))

		#Calculate PID parameters
		e_P = e_k
		e_I = self.E_k + e_k
		e_D = self.e_k_1 - e_k

		#The controlled angular velocity
		self.w = self.Kp*e_P + self.Ki*e_I + self.Kd*e_D
		
		#Updates
		self.E_k = e_I
		self.e_k_1 = e_k

		#Print statements for debugging
		rospy.loginfo('PID called, w is: ')
		rospy.loginfo(self.w)

		goal = np.array((data.x, data.y))
		rospy.loginfo('distance from goal: ')
		rospy.loginfo(np.linalg.norm(self.pose-goal))

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 quat2eulerZYX (self,q):
        euler = Vector3()
        tol = self.quat2eulerZYX.tolerance

        qww, qxx, qyy, qzz = q.w*q.w, q.x*q.x, q.y*q.y, q.z*q.z
        qwx, qxy, qyz, qxz= q.w*q.x, q.x*q.y, q.y*q.z, q.x*q.z
        qwy, qwz = q.w*q.y, q.w*q.z

        test = -2.0 * (qxz - qwy)
        if test > +tol:
           euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz)
           euler.y = +0.5 * pi
           euler.z = 0.0

           return euler

        elif test < -tol:
             euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz)
             euler.y = -0.5 * pi
             euler.z = tol

             return euler

        else:
           euler.x = atan2 (2.0*(qyz+qwx), qww-qxx-qyy+qzz)
           euler.y = asin (test)
           euler.z = atan2 (2.0*(qxy+qwz), qww+qxx-qyy-qzz)

           return euler

def speedsToReach( carrot, robotPose ) :
  """
  Compute robot commands for the next path point
  use 'follow the carrot' algorithm
  :param carrot: Point to reach
  :param robotPose: Robot collection storing robot position and orientation
  :return: speeds of the robots in a dictionnary
  """
  # robot angle
  angleRobot = atan2( getBearing()['Y'], getBearing()['X'] )
  # compute distance between the robot and the carrot
  distanceCarrot = computeDistance( carrot, robotPose['Position'] )
  # print "distance: ", distanceCarrot
  # compute the angle to the carrot
  angleCarrot = atan2( carrot['Y'] - robotPose['Position']['Y'], carrot['X'] - robotPose['Position']['X'] )
  angleToCarrot = angleCarrot - angleRobot
  if -pi > angleToCarrot :
    angleToCarrot = ( 2 * pi ) + angleToCarrot
  if pi < angleToCarrot :
      angleToCarrot = ( 2 * pi ) - angleToCarrot
  # print "angle: ", angleToCarrot
  # compute angular speed
  speeds = {}
  speeds['angular'] = ( angleToCarrot ) / radPerSec
  speeds['linear'] = ( speeds['angular'] * ( distanceCarrot / ( 2 * sin( angleToCarrot ) ) ) )
  
  if maxLinearSpeed < speeds['linear'] :
    speeds['linear'] = maxLinearSpeed
  if -maxLinearSpeed > speeds['linear'] :
    speeds['linear'] = -maxLinearSpeed
  return speeds

    def calc_ogp(self, params1, params2, freq):

        """
	   Calculates OGP values for both cores given best fit parameters
	   from a snap fit
	"""

	z_fact = -500.0/256.0
	true_zero = 0.0 * z_fact
	d_fact = 1e12/(2*np.pi*freq*1e6)

	z1 = z_fact * params1[0][0]
	sin1a = params1[0][1]
	cos1a = params1[0][2]
	amp1 = math.sqrt(sin1a**2 + cos1a**2)
	dly1 = d_fact*math.atan2(sin1a, cos1a)

	z2 = z_fact * params2[0][0]
	sin2a = params2[0][1]
	cos2a = params2[0][2]
	amp2 = math.sqrt(sin2a**2 + cos2a**2)
	dly2 = d_fact*math.atan2(sin2a, cos2a)

	avz = (z1+z2)/2.0
	avamp = (amp1+amp2)/2.0
	a1p = 100*(avamp-amp1)/avamp 
	a2p = 100*(avamp-amp2)/avamp 
	avdly = (dly1+dly2)/2.0

	ogp1 = (z1-true_zero, a1p, dly1-avdly)
	ogp2 = (z2-true_zero, a2p, dly2-avdly)

	return ogp1, ogp2

    def to_geographic(self, x, y):

        # Retrieve the locals.
        A, E, PI4, PI2, P0, M0, X0, Y0, P1, P2, m1, m2, t1, t2, t0, n, F, rho0 = self._locals

        # Subtract the false northing/easting.
        x = x - X0
        y = y - Y0

        # Calculate the Longitude.
        lon = math.atan2(x, rho0 - y) / n + M0

        # Estimate the Latitude.
        rho = math.sqrt(math.pow(x, 2.0) + math.pow(rho0 - y, 2.0))
        t = math.pow(rho / (A * F), 1.0 / n)
        lat = PI2 - (2.0 * math.atan2(t, 1.0))

        # Substitute the estimate into the iterative calculation
        # that converges on the correct Latitude value.
        while True:
            lat1 = lat
            es = E * math.sin(lat1)
            lat = PI2 - 2.0 * math.atan2(t * math.pow((1.0 - es) / (1.0 + es), E / 2.0), 1.0)
            if math.fabs(lat - lat1) < 2.0e-9:
                break

        # Return lat/lon in degrees.
        return math.degrees(lat), math.degrees(lon)

    def update_goals(self, dt):
        '''update the velocities to match the goals'''
        flagdist = collide.dist(self.pos, self.team.flag.pos)
        if self.flag and collide.rect2circle(self.team.base.rect, (self.pos, constants.TANKRADIUS)):
            self.team.map.scoreFlag(self.flag)

        if not self.flag and self.team.flag.tank is None and flagdist < config['puppy_guard_zone']:
            x,y = self.team.flag.pos
            rad = 1 # config['puppy_guard_zone']
            angle = math.atan2(self.pos[1]-y, self.pos[0]-x)
            npos = [math.cos(angle) * rad + self.pos[0], math.sin(angle) * rad + self.pos[1]]
            if not self.collision_at(npos, True):
                self.pos = npos
        

        ## return the flag once you get on "your side"
        # if self.flag and self.team.map.closest_base(self.pos) == self.team.base:
        #     self.team.map.scoreFlag(self.flag)

        self.hspeed += self.accelx
        self.vspeed += self.accely
        max = 30
        if collide.dist((0,0),(self.hspeed, self.vspeed)) > max:
            dr = math.atan2(self.vspeed, self.hspeed)
            self.hspeed = math.cos(dr) * max
            self.vspeed = math.sin(dr) * max

def matrix2angle(R):
    ''' compute three Euler angles from a Rotation Matrix. Ref: http://www.gregslabaugh.net/publications/euler.pdf
    Args:
        R: (3,3). rotation matrix
    Returns:
        x: yaw
        y: pitch
        z: roll
    '''
    # assert(isRotationMatrix(R))

    if R[2,0] !=1 or R[2,0] != -1:
        x = asin(R[2,0])
        y = atan2(R[2,1]/cos(x), R[2,2]/cos(x))
        z = atan2(R[1,0]/cos(x), R[0,0]/cos(x))
        
    else:# Gimbal lock
        z = 0 #can be anything
        if R[2,0] == -1:
            x = np.pi/2
            y = z + atan2(R[0,1], R[0,2])
        else:
            x = -np.pi/2
            y = -z + atan2(-R[0,1], -R[0,2])

    return x, y, z

	def angle (self, vector=None) :
		"""angle between two vectors
		to go from vector to self
		
		:warning: orientation in dimension 3 is not garanteed
		:param vector: the second vector
		:type vector: NVector
		:return: an angle in radian
		:rtype: float"""
		if self.dim()==2 :
			if vector is None : vector=axis(0,dim=2)
			else : vector=vector.get_normalized()
			y=vector.cross(self)#[2]
			x=self.dot(vector)
			return math.atan2(y,x)
		else : 
			if vector is None : vector=axis(0,self.dim())
			else : vector=vector.get_normalized()
			axis_normal=vector.cross(self)
			if axis_normal.norm()<1e-6 :
				if self.dot(vector)>0 : return 0.
				else : return math.pi
			
			axis_proj=axis_normal.cross(vector)
			axis_proj.normalize()
			y=self.dot(axis_proj)
			x=self.dot(vector)
			return math.atan2(y,x)

 def fusion_get_orientation(self):
     '''
     Fuses data from accelerometer and magnetometer.  Same algorithm as the
     original Adafruit 10-DOF function except pitch = -roll and roll = pitch
     since this makes much more sense for the board layout.
     Returns a tuple of (pitch, roll, heading)
     '''
     # Calculate pitch based only on accel
     (accelX, accelY, accelZ) = self.accel_get_raw()
     pitch = -atan2(accelY, accelZ)
     
     # Calculate roll based on pitch and accel
     if accelY * sin(-pitch) + accelZ * cos(-pitch) == 0:
         roll = pi / 2 if accelX > 0 else -pi / 2
     else:
         roll = atan(-accelX / (accelY * sin(-pitch) + accelZ * cos(-pitch)))
         
     # Calculate heading based on pitch, roll, and mag
     (magX, magY, magZ) = self.mag_get_raw()
     heading = atan2(magZ * sin(-pitch) - magY * cos(-pitch), 
                     magX * cos(roll) +
                     magY * sin(roll) * sin(-pitch) +
                     magZ * sin(roll) * cos(-pitch))
     
     return (pitch * 180 / pi, roll * 180 / pi, heading * 180 / pi)

 def _calc_det_angles_given_det_or_naz_constraint(
         self, det_constraint, naz_constraint, theta, tau, alpha):
     
     assert det_constraint or naz_constraint
     
     if det_constraint:
         # One of the detector angles is given                 (Section 5.1)
         det_constraint_name, det_constraint = det_constraint.items()[0]
         delta, nu, qaz = self._calc_remaining_detector_angles(
                          det_constraint_name, det_constraint, theta)
         naz_qaz_angle = _calc_angle_between_naz_and_qaz(theta, alpha, tau)
         naz = qaz - naz_qaz_angle
         
     elif naz_constraint: # The 'detector' angle naz is given:
         det_constraint_name, det_constraint = naz_constraint.items()[0]
         naz_name, naz = det_constraint_name, det_constraint
         assert naz_name == 'naz'
         naz_qaz_angle = _calc_angle_between_naz_and_qaz(theta, alpha, tau)
         qaz = naz - naz_qaz_angle
         nu = atan2(sin(2 * theta) * cos(qaz), cos(2 * theta))
         delta = atan2(sin(qaz) * sin(nu), cos(qaz))
         
     delta_nu_pairs = self._generate_detector_solutions(
                      delta, nu, qaz, theta, det_constraint_name)
     if not delta_nu_pairs:
         raise DiffcalcException('No detector solutions were found,'
             'please unconstrain detector limits')
     delta, nu = self._choose_detector_solution(delta_nu_pairs)
     
     return qaz, naz, delta, nu