import pandas as pd
import numpy as np
from scipy import spatial
from math import *


def rad2deg(rad):
	'''
	function to convert radian to degree
	'''
	degree = rad/2/np.pi * 360
	return(degree)

def kmToDIST(x):
	'''
	function to convert real distance in km to cartesian distance 
	'''
	R = 6371 # earth radius
	gamma = 2*np.arcsin(x/2./R) 
	
	dist = 2*R*rad2deg(sin(gamma / 2.))
	return(dist)

def to_Cartesian(lat, lng):
	'''
	function to convert latitude and longitude to 3D cartesian coordinates
	'''
	R = 6371 # radius of the Earth in kilometers

	x = R * cos(lat) * cos(lng)
	y = R * cos(lat) * sin(lng)
	z = R * sin(lat)
	return x, y, z

def haversine(lon1, lat1, lon2, lat2):
	"""
	Calculate the great circle distance between two points 
	on the earth (specified in decimal degrees)
	"""
	# convert decimal degrees to radians 
	lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])

	# haversine formula 
	dlon = lon2 - lon1 
	dlat = lat2 - lat1 
	a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
	c = 2 * asin(sqrt(a)) 
	r = 6371 # Radius of earth in kilometers. Use 3956 for miles
	
	return c * r

def calculate_initial_compass_bearing(pointA, pointB):
    """
    Calculates the bearing between two points.

    The formulae used is the following:
        θ = atan2(sin(Δlong).cos(lat2),
                  cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong))

    :Parameters:
      - `pointA: The tuple representing the latitude/longitude for the
        first point. Latitude and longitude must be in decimal degrees
      - `pointB: The tuple representing the latitude/longitude for the
        second point. Latitude and longitude must be in decimal degrees

    :Returns:
      The bearing in degrees

    :Returns Type:
      float
    """
    import math
    
    if (type(pointA) != tuple) or (type(pointB) != tuple):
        raise TypeError("Only tuples are supported as arguments")

    lat1 = math.radians(pointA[0])
    lat2 = math.radians(pointB[0])

    diffLong = math.radians(pointB[1] - pointA[1])

    x = math.sin(diffLong) * math.cos(lat2)
    y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1)
            * math.cos(lat2) * math.cos(diffLong))

    initial_bearing = math.atan2(x, y)

    # Now we have the initial bearing but math.atan2 return values
    # from -180° to + 180° which is not what we want for a compass bearing
    # The solution is to normalize the initial bearing as shown below
    initial_bearing = math.degrees(initial_bearing)
    compass_bearing = (initial_bearing + 360) % 360

    return compass_bearing

def dataprepossessing(path):
    df = pd.read_csv(path)
    df = df.sort_values(by='Date')
    #df = df.loc[:,['Date','lon','lat','app_id']]
    df = df.drop_duplicates(subset=['lon','lat'])
    df = df.reset_index(drop=True)
    df['direction'] = 0
    df['lon_1'] = df['lon'].shift(-1)
    df['lat_1'] = df['lat'].shift(-1)
    for i in range(len(df['Date'])):
        pointA = (df['lat'][i], df['lon'][i])
        pointB = (df['lat_1'][i], df['lon_1'][i])
        df['direction'][i] = calculate_initial_compass_bearing(pointA, pointB)
    df = df.drop(columns=['lon_1','lat_1'])
    df = df.drop((len(df)-1))
    
    #df = df.loc[:, ['lon','lat','app_id', 'accx','accy','accz', 'gyrox', 'gryoy','gyroz','movement','app_id']]
    
    df_approach = df.loc[:, ['lon','lat','direction','app_id']]
    df_movement = df.loc[:, ['accx','accy','accz', 'gyrox', 'gyroy','gyroz','movement']]
    
    approach_matrix = df_approach.as_matrix()
    movement_matrix = df_movement.as_matrix()
    
    return df, approach_matrix, movement_matrix