#-*- coding: utf-8 -*-

#####################################################################
# SPIIRAS OOGIS lab property                                        #
# Project: Area calculation using gps data (longitude and latitude) #
# Author:  Michael Verkhovykh <*****************>                   #
# License: GNU LGPL                                                 #
#####################################################################

from math import *

# WGS 84 geoid constants
A = 6378.137 # semi-major axis
B = 6356.752314245 # semi-minor axis
F = 1/298.257223563 # flattening
e = sqrt(1-(B*B)/(A*A)) # first numerical eccentricity of the ellipsoid

def Geo2ECEF(data):
    """Function to transform GPS data (latitude ond longitude) to ECEF format (Earth-centred)"""
    new_data = []
    for a in data:
        psi = sqrt(1 - e*e*sin(a[0]*pi/180)*sin(a[0]*pi/180) )
        x = (A/psi)*cos(a[0]*pi/180)*cos(a[1]*pi/180)
        y = (A/psi)*cos(a[0]*pi/180)*sin(a[1]*pi/180)
        z = (A*(1-e*e)/psi)*sin(a[0]*pi/180)
        new_data.append( (x, y, z) )
    return new_data

def dot(v1, v2):
    """Calculating dot product for 2 3-dimensioned vectors"""
    return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]

def cross(a, b):
    c = [a[1]*b[2] - a[2]*b[1],
         a[2]*b[0] - a[0]*b[2],
         a[0]*b[1] - a[1]*b[0]]
    return c

#import math
data = [
  (60+2.4/60, 25+41.29/60),
  (60+7.35/60, 26+17.13/60),
  (59+52.11/60, 26+25.19/60),
  (59+47.8/60, 25+49.08/60),
]

print "Following way-points:", data

ndata = Geo2ECEF(data)

print "Data in ECEF format:", ndata
#uncomment to understand, that everuthing is working :)
#for a in ndata:
#   print a, sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2])

area = 0;
area = sqrt(dot(cross(ndata[len(ndata)-1], ndata[0]), cross(ndata[len(ndata)-1], ndata[0])))
for i in range(len(ndata)-1):
    area += 0.5*sqrt(dot(cross(ndata[i], ndata[i+1]), cross(ndata[i], ndata[i+1])))
print area

#print area