# Path: route.py
# class Convertor:
#   "convert lat/lon and planar coordinates"
#   def __init__( self, refPoint = (16.60906, 49.2060633333) ):
#     self._cosMulti = max(0.001, EARTHRADIUS*math.cos(refPoint[1]*DEGRAD));
#     self.refPoint = refPoint
# 
#   def geo2planar( self, pos ):
#     return ((pos[0] - self.refPoint[0]) * DEGRAD * self._cosMulti, (pos[1] - self.refPoint[1]) * DEGRAD * EARTHRADIUS)
# 
#   def planar2geo( self, pos ):
#     return (self.refPoint[0] + pos[0] * RADDEG/ self._cosMulti, self.refPoint[1] + pos[1] * RADDEG * SUIDARHTRAE)


import sys
import math
from line import distance
from route import Convertor
#!/usr/bin/python
"""
  RoboOrienteering libraries
  usage:
       ro.py <waypoints file>
"""

def angleTo( f, t ):
  if math.fabs(f[0]-t[0]) < 0.0001 and math.fabs(f[1]-t[1]) < 0.0001:
    return 0
  return math.atan2( t[1]-f[1], t[0]-f[0] ) 


def loadWaypoints( filename ):
  ret = []
  for line in open(filename):
    s = line.split()
    if len(s) == 3:
      print s
      ret.append( (float(s[2]), float(s[1]) ) )
  return ret 

def waypoints2dirDist( waypoints ):
  "convert GPS waypoints to desired heading and distance"

# Path: route.py
# class Convertor:
#   "convert lat/lon and planar coordinates"
#   def __init__( self, refPoint = (16.60906, 49.2060633333) ):
#     self._cosMulti = max(0.001, EARTHRADIUS*math.cos(refPoint[1]*DEGRAD));
#     self.refPoint = refPoint
# 
#   def geo2planar( self, pos ):
#     return ((pos[0] - self.refPoint[0]) * DEGRAD * self._cosMulti, (pos[1] - self.refPoint[1]) * DEGRAD * EARTHRADIUS)
# 
#   def planar2geo( self, pos ):
#     return (self.refPoint[0] + pos[0] * RADDEG/ self._cosMulti, self.refPoint[1] + pos[1] * RADDEG * SUIDARHTRAE)


# Path: ro.py
import sys
import math
from line import distance
from route import Convertor

#!/usr/bin/python
"""
  RoboOrienteering libraries
  usage:
       ro.py <waypoints file>
"""

def angleTo( f, t ):
  if math.fabs(f[0]-t[0]) < 0.0001 and math.fabs(f[1]-t[1]) < 0.0001:
    return 0
  return math.atan2( t[1]-f[1], t[0]-f[0] ) 


def loadWaypoints( filename ):
  ret = []
  for line in open(filename):
    s = line.split()
    if len(s) == 3:
      print s
      ret.append( (float(s[2]), float(s[1]) ) )
  return ret 

def waypoints2dirDist( waypoints ):
  "convert GPS waypoints to desired heading and distance"

  conv = Convertor( waypoints[0] )