def fromwgs84(lat, lng, pkm=False):
    """
    Convert coordintes from WGS84 to TWD97

    pkm true for Penghu, Kinmen and Matsu area
    The latitude and longitude can be in the following formats:
        [+/-]DDD°MMM'SSS.SSSS" (unicode)
        [+/-]DDD°MMM.MMMM' (unicode)
        [+/-]DDD.DDDDD (string, unicode or float)
    The returned coordinates are in meters
    """

    _lng0 = lng0pkm if pkm else lng0

    lat = radians(todegdec(lat))
    lng = radians(todegdec(lng))

    t = sinh((atanh(sin(lat)) - 2*pow(n,0.5)/(1+n)*atanh(2*pow(n,0.5)/(1+n)*sin(lat))))
    epsilonp = atan(t/cos(lng-_lng0))
    etap = atan(sin(lng-_lng0) / pow(1+t*t, 0.5))

    E = E0 + k0*A*(etap + alpha1*cos(2*1*epsilonp)*sinh(2*1*etap) + 
                          alpha2*cos(2*2*epsilonp)*sinh(2*2*etap) +
                          alpha3*cos(2*3*epsilonp)*sinh(2*3*etap))
    N = N0 + k0*A*(epsilonp + alpha1*sin(2*1*epsilonp)*cosh(2*1*etap) +
                              alpha2*sin(2*2*epsilonp)*cosh(2*2*etap) +
                              alpha3*sin(2*3*epsilonp)*cosh(2*3*etap))

    return E*1000, N*1000

   def wmplugin_exec(self, m):
	
	axes = [None, None, None, None]


	self.acc = [self.NEW_AMOUNT*(new-zero)/(one-zero) + self.OLD_AMOUNT*old
	       for old,new,zero,one in zip(self.acc,m,self.acc_zero,self.acc_one)]
	a = math.sqrt(sum(map(lambda x: x**2, self.acc)))

	roll = math.atan(self.acc[cwiid.X]/self.acc[cwiid.Z])
	if self.acc[cwiid.Z] <= 0:
		if self.acc[cwiid.X] > 0: roll += math.pi
		else: roll -= math.pi

	pitch = math.atan(self.acc[cwiid.Y]/self.acc[cwiid.Z]*math.cos(roll))

	axes[0] = int(roll  * 1000 * self.Roll_Scale)
	axes[1] = int(pitch * 1000 * self.Pitch_Scale)

	if (a > 0.85) and (a < 1.15):
		if (math.fabs(roll)*(180/math.pi) > 10) and \
		   (math.fabs(pitch)*(180/math.pi) < 80):
			axes[2] = int(roll * 5 * self.X_Scale)

		if (math.fabs(pitch)*(180/math.pi) > 10):
			axes[3] = int(pitch * 10 * self.Y_Scale)

	return math.degrees(pitch), math.degrees(roll)

	def execute(self, context):
		lightOffset = 20
		lightHeight = 20
		scene = context.scene
		# delete active object if it is a mesh
		active = context.object
		if active and active.type=="MESH":
			bpy.ops.object.delete()
		# getting width and height of the footprint
		w, h = [float(i) for i in self.size.split("x")]
		# add lights
		rx = math.atan((h+lightOffset)/lightHeight)
		rz = math.atan((w+lightOffset)/(h+lightOffset))
		def lamp_add(x, y, rx, rz):
			bpy.ops.object.light_add(
				type="SUN",
				location=((x,y,lightHeight)),
				rotation=(rx, 0, rz)
			)
			context.active_object.data.energy = 0.5
		lamp_add(w+lightOffset, h+lightOffset, -rx, -rz)
		lamp_add(-w-lightOffset, h+lightOffset, -rx, rz)
		lamp_add(-w-lightOffset, -h-lightOffset, rx, -rz)
		lamp_add(w+lightOffset, -h-lightOffset, rx, rz)
		
		create_rectangle(context, w, h)
		align_view(context.object)
		
		return {"FINISHED"}

    def lift_drag(self, state_vector, t, vec = [-1, 0, 1]):
        """
        vec = [-1, 0, 1]
        Drag versor in capsule coordinate system
        """
        x, y, vx, vy = state_vector

        # kąt promienia wodzącego
        tau_r = atan(-x/y)

        # Flight Patch Angle
        FPA = atan(vy/vx)

        # convert time to positive numbers
        t = t + self.SUFR_duration_total

        # Bank change time (from charts, approx)
        roll_time = 10          # s

        #~ if t <= roll_time:
            #~ AoA = self.AoA_at_SUFR_start*(1-t*(1./roll_time))
        #~ elif t > roll_time:
            #~ AoA = 0.
        AoA = self.AoA_function(t)*cos(self.bank_function(t))
        # tau_r > 0; FPA < 0, AoA > 0
        # everything should be < 0
        Ex, Ey, __ = np.matrix(vec).dot(self.obrot_o_kat(-tau_r +FPA -AoA)).tolist()[0]
        return Ex, Ey

 def gazePix(self,anglex,angley):
     """Converts gaze angle to monitor pixel"""
     alphax = math.degrees(math.atan(self.ledx/self.monitordistance))
     pixx = self.deg2pix(anglex-alphax)
     alphay = math.degrees(math.atan(self.ledy/self.monitordistance))
     pixy = self.deg2pix(angley-alphay)
     return pixx,pixy

    def trouverInfoRobot(self, contourGauche, contourDroit):
        centreGauche = self.trouverCentre(contourGauche)
        centreDroit = self.trouverCentre(contourDroit)
        centreRobot = (int(round((centreDroit[0]+centreGauche[0])/2)), int(round((centreDroit[1]+centreGauche[1])/2)))
        deltaX = centreDroit[0]-centreGauche[0]
        deltaY = -1*(centreDroit[1]-centreGauche[1])
        if not deltaX == 0:
            pente = deltaY/deltaX

        if deltaY == 0 and deltaX < 0:
            angle = 180
        elif deltaY == 0 and deltaX > 0:
            angle = 0
        elif deltaX == 0 and deltaY > 0:
            angle = 90
        elif deltaX == 0 and deltaY < 0:
            angle = 270
        elif deltaX > 0 and deltaY > 0:
            angle = int(round(math.degrees(math.atan(pente))))
        elif deltaX > 0 and deltaY < 0:
            angle = 360 + int(round(math.degrees(math.atan(pente))))
        elif deltaX < 0:
            angle = 180 + int(round(math.degrees(math.atan(pente))))

        angle += 90
        if angle >= 360:
            angle -= 360

        return centreRobot, angle

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 mu_2_1(ay, by):
    return (1.0 / (2.0 * by)) * \
            (2.0 * ay * \
                    (atan((1.0 - ay) / by) + atan((1.0 + ay) / by)) + \
              by * \
                    (log((-1.0 + ay) ** 2 + by ** 2) - \
                     log((1.0 + ay) ** 2 + by ** 2)))

 def GoToPoint(self):
     self.brain.output.printf("go to point")
     PointY = self.B_Y + self.B_Y * (self.B_X - self.X_M)/(self.B_X - self.F_X)
     deltaX = self.R_X - self.X_M
     deltaY = abs(PointY - self.R_Y)
     deltaTheta = R_Theta - (pi - atan(deltaY/deltaX)) 
     dist = sqrt(deltaY * deltaY + deltaX * deltaX)
     forward = 0
     left = 0
     turnCCW = 0
     if dist > self.Radius:
         if R_Theta < pi - atan((deltaY + self.Radius)/deltaX):
             turnCCW = deltaTheta * sin(deltaTheta)#turnCCW=CLIP(deltaTheta,DEG_TO_RAD(-20),DEG_TO_RAD(20))
             self.brain.output.printf2('GoToPoint------turnCCW:::::::',turnCCW)
             self.brain.setWalk(1,0,0,turnCCW)
         elif R_Theta > pi - atan((deltaY - self.Radius)/deltaX):
             turnCCW = -deltaTheta * sin(deltaTheta)#rurnCCW=CLIP(-deltaTheta,DEG_TO_RAD(-20),DEG_TO_RAD(20))
             self.brain.output.printf2('GoToPoint------turnCCW:::::::',turnCCW)
             self.brain.setWalk(1,0,0,turnCCW)
         else:
             self.brain.setWalk(1,0,0,0)
         forward = dist * sin(dist)#or forward = 0.02 can be adopt
         left = dist * sin(dist)#or left = 0.02 can be adopt
         turnCCW = 0
         self.brain.setWalk(1,forward,left,turnCCW)

def Meter2px(h,vd,vd1,hd,nrows,ncols):
    ### real scale (unit : meter)
    #h  : building size
    #vd : distance between buliding to the bottom of the image
    #vd1: distance between bottom to up of the image
    #hd : horizontal distancd
    
    ### pixels based
    # nrows
    # ncols


    theta = math.atan(1.*vd/h) #radius 
    z=(h**2+vd**2)**0.5
    alpha = pi/2-theta
    beta = math.atan((vd+vd1)*1./h)-theta

    ydp = beta/ncols
    xdp = beta/nrows

    ylen = np.zeros(ncols)
    xlen = np.ones(nrows)*hd/nrows

    distance = vd
    for i in range(ncols):
        
        gamma = (theta+ydp*(1+i))
        pylen = h*math.tan(gamma)-distance # pixel length
        ylen[i] = pylen
        distance = distance+pylen

    print ylen[-1]/ylen[0]
    return xlen,ylen[::-1]

def mu_6_0(ay, by):
    return ((-3.0 * (-1.0 + ay)**3.0 * by - 5.0 * (-1.0 + ay) * by**3.0)/\
            ((-1.0 + ay)**2.0 + by**2.0)**2.0 + \
            (3.0 * (1.0 + ay)**3.0 * by + 5.0 * (1.0 + ay) * by ** 3.0)/\
            ((1.0 + ay)**2.0 + by**2.0)**2.0 -
            3.0 * atan((-1.0 + ay)/by) + 3.0 * atan((1.0 + ay)/by))/\
                    (8.0 * by**5.0)

def m2nu(m):
    if m < 1:
        raise ValueError('Mach number should be greater than or equal to 1')
    a = (GAMMA+1) / (GAMMA-1)
    b = m**2 - 1
    c = a**-1 * b
    return sqrt(a) * atan(sqrt(c)) - atan(sqrt(b))

    def __init__(self, central_longitude=0.0, satellite_height=35785831,
                 false_easting=0, false_northing=0, globe=None):
        proj4_params = [('proj', 'geos'), ('lon_0', central_longitude),
                        ('lat_0', 0), ('h', satellite_height),
                        ('x_0', false_easting), ('y_0', false_northing),
                        ('units', 'm')]
        super(Geostationary, self).__init__(proj4_params, globe=globe)

        # TODO: Factor this out, particularly if there are other places using
        # it (currently: Stereographic & Geostationary). (#340)
        def ellipse(semimajor=2, semiminor=1, easting=0, northing=0, n=200):
            t = np.linspace(0, 2 * np.pi, n)
            coords = np.vstack([semimajor * np.cos(t), semiminor * np.sin(t)])
            coords += ([easting], [northing])
            return coords

        # TODO: Let the globe return the semimajor axis always.
        a = np.float(self.globe.semimajor_axis or 6378137.0)
        b = np.float(self.globe.semiminor_axis or 6378137.0)
        h = np.float(satellite_height)
        max_x = h * math.atan(a / (a + h))
        max_y = h * math.atan(b / (b + h))

        coords = ellipse(max_x, max_y,
                         false_easting, false_northing, 60)
        coords = tuple(tuple(pair) for pair in coords.T)
        self._boundary = sgeom.polygon.LinearRing(coords)
        self._xlim = self._boundary.bounds[::2]
        self._ylim = self._boundary.bounds[1::2]
        self._threshold = np.diff(self._xlim)[0] * 0.02

def get_vec_ang(vec):
	'''
	Get the angle of a vector in [0,360).
	Input: a 2D vector
	Output: [0, 360)
	'''
	if (0 == vec[0]): # vx = 0
		if (vec[1] > 0):
			return 90
		elif (vec[1] < 0):
			return 270
		else:
			return 0
	else: # vx != 0
		if(0 == vec[1]): # vy = 0
			if(vec[0] > 0):
				return 0
			else:
				return 180
		else: # vx != 0, vy != 0
			temp = math.fabs(vec[1]/vec[0])
			if ( vec[0]>0 and vec[1]>0 ): # 1st quadrant 
				return math.atan(temp) * 180 / math.pi
			elif ( vec[0]<0 and vec[1]>0 ): # 2
				return (math.pi-math.atan(temp)) * 180 / math.pi
			elif ( vec[0]<0 and vec[1]<0 ): # 3
				return (math.pi+math.atan(temp)) * 180 / math.pi
			else: # 4
				return (2*math.pi-math.atan(temp)) * 180 / math.pi

def generate_wind(nodes, cells, prevailing_wind_bearing=270, average_wind_speed=7.0):
    import noise
    import math
    import numpy as np
    for n in nodes:
        wind_bearing = (int(round(180.0 * noise.pnoise3(n.x, n.y, n.z))) + 360) % 360  # in degrees
        wind_strength = (8 * noise.pnoise3(n.x, n.y, n.z)) + 16.0  # kmph

        # move the generated random wind bearing towards the prevailing wind a little
        wind_vector = np.add(np.array([wind_strength * math.cos(math.radians(90 - wind_bearing)),
                                       wind_strength * math.cos(math.radians(wind_bearing))]),
                             np.array([average_wind_speed * math.cos(math.radians(90 - prevailing_wind_bearing)),
                                       average_wind_speed * math.cos(math.radians(prevailing_wind_bearing))]))

        wind_strength = math.sqrt(math.pow(wind_vector[0], 2) + math.pow(wind_vector[1], 2)) #  sqrt((x2-x1)^2 + (y2-y1)^2) = vector magnitude
        wind_bearing = int(round(math.degrees(math.atan(wind_vector[1]/wind_vector[0])))) # tan-1((y2-y1)/x2-x1)) = vector bearing

        n.set_feature(['wind', 'bearing'], wind_bearing)
        n.set_feature(['wind', 'strength'], wind_strength)
        n.set_feature(['wind', 'vector', 'x'], wind_vector[0])
        n.set_feature(['wind', 'vector', 'y'], wind_vector[1])
        n.wind = Wind(wind_bearing, wind_strength, wind_vector)
    for c in cells:
        wind_vector = np.sum(np.array([n.wind.vector for n in c.nodes]), axis=0)
        wind_strength = math.sqrt(math.pow(wind_vector[0], 2) + math.pow(wind_vector[1], 2))  #  sqrt((x2-x1)^2 + (y2-y1)^2) = vector magnitude
        wind_bearing = int(round(math.degrees(math.atan(wind_vector[1]/wind_vector[0]))))  # tan-1((y2-y1)/x2-x1)) = vector bearing

        c.wind = Wind(wind_bearing, wind_strength, wind_vector)

    def TurnToBall(self):
        self.brain.output.printf("TurnToBall")
        if self.brain.ball.confidence > 0:
            if self.gcData.Tcolor() == Constants.TEAM.RED:
                alpha = self.R_Theta - atan((self.B_Y - self.R_Y)/(self.B_X - self.R_X))
                if abs(alpha) > self.turnDeg:#self.turnDeg is the minimum degree that needs to turn
                    if alpha < 2*pi - alpha:
                        turnCCW = -alpha * sin(alpha)
                        self.brain.setWalk(1,0,0,turnCCW)
                    else:
                        turnCCW = (2*pi - alpha)*sin(2*pi - alpha)
                        self.brain.setWalk(1,0,0,turnCCW)

            else:
                alpha = pi - self.R_Theta -atan((self.B_Y - self.R_Y)/(self.R_X - self.R_X - self.B_X))
                if abs(alpha) >self.turnDeg:
                    if alpha <2*pi - alpha:
                        turnCCW = alpha * sin(alpha)
                        self.brain.setWalk(1,0,0,turnCCW)
                    else:
                        turnCCW = -alpha*sin(-alpha)
                        self.brain.setWalk(1,0,0,turnCCW)

        else:
            FindBall(self.brain)

def followCentre(data,desired_trajectory):
	global alpha

	a = getRange(data,130)
	b = getRange(data,179.9)
	swing = math.radians(50)
	print "center distances: ", a, b
	alpha = -math.atan((a*math.cos(swing)-b)/(a*math.sin(swing)))
	print "Alpha left",math.degrees(alpha)
	curr_dist1 = b*math.cos(alpha)
	future_dist1 = curr_dist1-car_length*math.sin(alpha)



	a = getRange(data,50)
	b = getRange(data,0)
	swing = math.radians(50)
	alpha = math.atan((a*math.cos(swing)-b)/(a*math.sin(swing)))
	print "Alpha right",math.degrees(alpha)
	curr_dist2 = b*math.cos(alpha)

	future_dist2 = curr_dist2+car_length*math.sin(alpha)

	desired_trajectory = (future_dist1 + future_dist2)/2

	print "dist 1 : ",future_dist1
	print "dist 2 : ",future_dist2
	# print "dist : ",future_dist
	error = future_dist1 - future_dist2
	print "Error : ",error
	return error, curr_dist2-curr_dist1

 def getInfo(self):
     """
     Return the data retrieved from the wiimote properly formatted and ordered
     """
     state = self.wm.state
     (xi, yi, zi), ir_src, buttons = state['acc'], state['ir_src'], state['buttons']
     
     x = float(xi)
     y = float(yi)
     z = float(zi)
     
     #Weight the accelerations according to calibration data and
     #center around 0
     a_x = (x - self._accel_calib[0])/(self._accel_calib[4]-self._accel_calib[0])
     a_y = (y - self._accel_calib[1])/(self._accel_calib[5]-self._accel_calib[1])
     a_z = (z - self._accel_calib[2])/(self._accel_calib[6]-self._accel_calib[2])
     
     try:
         roll = math.atan(float(a_x)/float(a_z))
         if a_z<=0:
                 if (a_x>0):
                         roll -= math.pi
                 else:
                         roll += math.pi
         roll = -roll
         pitch = math.atan(a_y/a_z*math.cos(roll))
         accel = math.sqrt(math.pow(a_x,2)+math.pow(a_y,2)+math.pow(a_z,2))
     
         return pitch, roll, accel, (a_x, a_y, a_z), ir_src, buttons 
     except ZeroDivisionError:
         return 0,0,0,(0,0,0), 0, 0

	def testEvent(self,timestamp,ID,x,y,z):
		"""tests to see if the current wiimote position needs to trigger an event.
		creates and sends the wii event if needed"""	
		x = -1 * x#flips x acceleration
		if z == 0:#since tangent will be x/z, z = 0 needs to be slightly adjusted to avoid DIVIDE BY 0
			z = .0000000001
		if abs(z) < ParserSettings.ROLL_PITCH_LOCKOUT_THRESHOLD and abs(x) < ParserSettings.ROLL_PITCH_LOCKOUT_THRESHOLD:
			#checks if there may be a roll or pitch occuring, where the accelerometers are spiking.
			#function doesnt test for a roll or pitch event if there is a flick occuring.
			#intent is to prevent false positives with roll and pitch events as much as possible.
			roll = math.atan(float(x)/float(z))#roll is calculated comparing calibrated x and z acceleration values
			pitch = math.atan(float(y)/float(z))*math.cos(roll)#pich is compared with calibrated y and z, modified by the roll
		
			if (roll - self.currentRoll) > ParserSettings.ROLL_THRESHOLD:
				#tests to see if the roll has changed enough to trigger an event
				triggeredEvent = WiiEvent.WiiEvent(ID,'Roll',1,timestamp)
				triggeredEvent.fireEvent()
				self.currentRoll = self.currentRoll + ParserSettings.ROLL_THRESHOLD
			elif(self.currentRoll - roll) > ParserSettings.ROLL_THRESHOLD:
				#tests for roll in other direction
				triggeredEvent = WiiEvent.WiiEvent(ID,'Roll',-1,timestamp)
				triggeredEvent.fireEvent()
				self.currentRoll = self.currentRoll - ParserSettings.ROLL_THRESHOLD
	
			if (pitch - self.currentPitch) > ParserSettings.PITCH_THRESHOLD:
				#tests pitch in same manner as role, relative to standard threshold
				triggeredEvent = WiiEvent.WiiEvent(ID,'Pitch',1,timestamp)
				triggeredEvent.fireEvent()
				self.currentPitch = self.currentPitch + ParserSettings.PITCH_THRESHOLD
			elif(self.currentPitch - pitch) > ParserSettings.PITCH_THRESHOLD:
				#tests other direction of pitch
				triggeredEvent = WiiEvent.WiiEvent(ID,'Pitch',-1,timestamp)
				triggeredEvent.fireEvent()
				self.currentPitch = self.currentPitch - ParserSettings.PITCH_THRESHOLD

def cal_angle(x_next,x_curr,y_next,y_curr,x_hori,y_vert,northAt):
  # 1st Quadrant
  if x_next > x_curr and y_next > y_curr:
    angle = (90 - math.degrees(math.atan(y_vert/x_hori)))
  # 2nd Quadrant
  elif x_next > x_curr and y_next < y_curr:
    angle = math.degrees(math.atan(y_vert/x_hori)) + 90
  # 3rd Quadrant
  elif x_next < x_curr and y_next < y_curr:
    angle = 270 - math.degrees(math.atan(y_vert/x_hori))
  # 4th Quadrant
  elif x_next < x_curr and y_next > y_curr:
    angle = math.degrees(math.atan(y_vert/x_hori)) + 270
  elif x_next == x_curr and y_next > y_curr:
    angle = 0
  elif x_next == x_curr and y_next < y_curr:
    angle = 180
  elif x_next > x_curr and y_next == y_curr:
    angle = 90
  elif x_next < x_curr and y_next == y_curr:
    angle = 270    

  angle = angle - northAt
  
  if angle >= 360:
      angle -= 360
  elif angle < 0:
    angle += 360

  return angle