#!/usr/bin/python

#
# Copyright (c) 2007,2008,2009 MBARI
# MBARI Proprietary Information.  All Rights Reserved
#

from math import *

# constants
EARTH_RADIUS = 6371000.0
EARTH_RADIUS_INVERTED = 1.0 / 6371000.0

class Location:

  def __init__(self, lat = 0.0, lon = 0.0, unit="radian"):
    if unit.lower() != "radian":
      self.lat = radians(lat)
      self.lon = radians(lon)
    else:
      self.lat = lat
      self.lon = lon

  def getLat(self, unit="radian"):
    if unit.lower() != "radian":
      return degrees(self.lat)
    return self.lat
    
  def getLon(self, unit="radian"):
    if unit.lower() != "radian":
      return degrees(self.lon)
    return self.lon

  def setLat(self, lat, unit="radian"):
    if unit.lower() != "radian":
      self.lat = radians(lat)
    else:
      self.lat = lat
  
  def setLon(self, lon, unit="radian"):
    if unit.lower() != "radian":
      self.lon = radians(lon)
    else:
      self.lon = lon

  def __add__(self, other):
    lat = self.lat + other.lat
    lon = self.lon + other.lon
    return Location(lat, lon)
     
  def __sub__(self, other):
    lat = self.lat - other.lat
    lon = self.lon - other.lon
    return Location(lat, lon)

  def getBearing(self, other):
    return Location.s_getBearing(self.lat, self.lon, \
        other.lat, other.lon)

  def getDistance(self, other):
    return Location.s_getDistance(self.lat, self.lon, \
        other.lat, other.lon)

  def atBearing(self, bearing, distance):
    (lat, lon) = Location.s_atBearing(bearing, distance, \
        self.lat, self.lon)
    return Location(lat, lon)

  def isWithinDistance(self, other, distance):
    return Location.s_isWithinDistance(self.lat, self.lon, \
        other.lat, other.lon, distance)
  
  def hasXPerpLine(self, start, target):
    return Location.s_hasXPerpLine(start.lat, start.lon, \
        self.lat, self.lon, target.lat, target.lon)

  # get the bearing to lat2, lon2 from lat1, lon1
  @staticmethod
  def s_getBearing(lat1, lon1, lat2, lon2):
    y = sin(lon2 - lon1 ) * cos(lat2)
    x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1);
    return atan2(y, x);

  # get the distance between lat1, lon1 and lat2, lon2
  @staticmethod
  def s_getDistance(lat1, lon1, lat2, lon2):
    return EARTH_RADIUS * acos(sin(lat1) * sin(lat2) + cos(lat1) * \
           cos(lat2) * cos(lon2 - lon1))

  # returns the resulting (lat, lon) position if
  # one starts at position lat, lon towards bearing
  # for distance specified
  @staticmethod
  def s_atBearing(bearing, distance, lat, lon):
    r = distance * EARTH_RADIUS_INVERTED
    rlat = asin(sin(lat) * cos(r) + cos(lat) * sin(r) * cos(bearing));
    rlon = lon + atan2(sin(bearing) * sin(r) * cos(lat), 
           cos(r) - sin(lat) * sin(rlat));
    return rlat, rlon

  # is lat1, lon1, withing dist of lat2, lon2?
  @staticmethod
  def s_isWithinDistance(lat1, lon1, lat2, lon2, dist):
    distance = Location.s_getDistance(lat1, lon1, lat2, lon2)
    if distance <= dist:
      return True
    return False
  
  # has the current lat, lon position crossed the
  # the perpendicular line of the target waypoints 
  # Tethys uses this mechanism for some of its navigation
  # slat, slon - starting lat, lon
  # clat, clon - current lat, lon
  # tlat, tlon - target lat, lon
  # We round and use >= because we have more accuracy
  # when calculating the perpendicular line than 
  # we do for the clat and clon values we get from the slate
  @staticmethod
  def s_hasXPerpLine(slat, slon, clat, clon, tlat, tlon):
    isOverline = False
    if (tlat != slat):
      perpSlope = -(tlon - slon) / (tlat - slat)
      isOverLine = round(clat, 7) >= \
          round(tlat + (clon - tlon) * perpSlope, 7)
    else:
      isOverLine = round(clon, 7) >= round(tlon, 7)
    return isOverLine