import math
from math import cos, sin, acos 

def angle(P1,P2):
   ang={'rad':0.,'deg':0.}	
   if ((P1['x']==P2['x'])):
      if ((P1['y']==P2['y'])):
	 return 0
      else :
	 if ((P1['y']<P2['y'])):
            ang['rad']=math.pi*0.5
            ang['deg']=180.*ang['rad']/math.pi
	    return ang['rad']
         else :
            ang['rad']=math.pi*1.5
            ang['deg']=180.*ang['rad']/math.pi
            return ang['rad']
   tan12=(P2['y']-P1['y'])/(P2['x']-P1['x'])
   dx=P2['x']-P1['x']
   dy=P2['y']-P1['y']
   cos12=dx/math.sqrt(dx*dx+dy*dy)
#   print 'cos12',cos12
   
   ang['rad']=math.acos(cos12)
   if (ang['rad']<0) :
      ang['rad']=ang['rad']+2.*math.pi
   if (ang['rad']>2.*math.pi) :
      ang['rad']=ang['rad']-2.*math.pi
   if (P2['y']<P1['y']) :
      ang['rad']=-ang['rad']+2.*math.pi

       
   ang['deg']=180.*ang['rad']/math.pi
   print 'ang:',ang['deg'],ang['rad']
   return ang['rad']

def  distance(lat1,lon1,lat2,lon2):
# Get lat,lon in degree and return distance in KM	
   R_e=6378.161  # Km
   FAC=math.pi/180.
   lat11=FAC*lat1
   lon11=FAC*lon1
   lat22=FAC*lat2
   lon22=FAC*lon2
   cos_distance= math.sin(lat11)*math.sin(lat22) +                 math.cos(lat11)*math.cos(lat22)*math.cos(lon22-lon11)
   print "Cos Distance:",cos_distance
   if ((lat1==lat2)&(lon1==lon2)) :
      rad_distance=0.
   else :
      rad_distance=math.acos( cos_distance )
     
   dist=rad_distance*R_e
   return dist
   
def AC_error(o_lat1,o_lon1,o_lat2,o_lon2,f_lat2,f_lon2):
   R_e=6378.161  # Km
   FAC=math.pi/180.
   o1_o2=distance(o_lat1,o_lon1,o_lat2,o_lon2)
   o2_f2=distance(o_lat2,o_lon2,f_lat2,f_lon2)
   o1_f2=distance(o_lat1,o_lon1,f_lat2,f_lon2)
 
   a_o1_o2=o1_o2/R_e 
   a_o2_f2 =o2_f2/R_e 
   a_o1_f2=o1_f2/R_e 

   temp=sin(a_o1_o2)*sin(a_o2_f2)
   cos_o2=(cos(a_o1_f2) - cos(a_o1_o2)*cos(a_o2_f2)) / temp
   a_o2=acos(cos_o2)
   angle=math.pi - a_o2
# determine the sign (>0: right, <0:-left)
   sign= - (o_lon2-o_lon1)*(f_lat2-o_lat2) + (o_lat2-o_lat1)*(f_lon2-o_lon2) 

   if (sign<0):
     angle=-angle

   e_at=o2_f2*cos(angle)
   e_ct=o2_f2*sin(angle)   
   return (e_at,e_ct)