#####################################################
# @module    file_utils.py                          #
# @brief     Common unit handling tools             #
# @author    Original Code form multiple authors    #
# @date      Sept 2020                              #
# @copyright Racelogic Ltd., 2020                   #
#####################################################

from math import cos, sin, sqrt, atan2

class Location_MinE5:
    def __init__(self, minE5=None):
        if not minE5 is None:
            self.lat = minE5.lat
            self.lng = minE5.lng
            self.alt_m = minE5.alt_m
        else:
            self.lat = None
            self.lng = None
            self.alt_m = None

class Location_MinE8:
    def __init__(self, minE8=None):
        if not minE8 is None:
            self.lat = minE8.lat
            self.lng = minE8.lng
            self.alt_m = minE8.alt_m
        else:
            self.lat = None
            self.lng = None
            self.alt_m = None

class Location_DegE9:
    def __init__(self, degE9=None):
        if not degE9 is None:
            self.lat = degE9.lat
            self.lng = degE9.lng
            self.alt_m = degE9.alt_m
        else:
            self.lat = None
            self.lng = None
            self.alt_m = None                     

class Location_Rads:
    def __init__(self, minE5=None, minE8=None, degE9=None,file=None):
        if not minE5 is None:
            self.lat = Unit_Conv.minE5_to_rad(minE5.lat)
            self.lng = Unit_Conv.minE5_to_rad(minE5.lng)
            self.alt_m = minE5.alt_m
            if file is not None:
                file.write('MinE5 lat:'+str(self.lat)+' lng:'+str(self.lng)+' alt:'+str(self.alt_m)+'\r\n')
            #print('MinE5 lat:',self.lat,' lng:',self.lng,' alt:',self.alt_m)
        elif not minE8 is None:
            self.lat = Unit_Conv.minE8_to_rad(minE8.lat)
            self.lng = Unit_Conv.minE8_to_rad(minE8.lng)
            self.alt_m = minE8.alt_m
            if file is not None:
                file.write('MinE8 lat:'+str(self.lat)+' lng:'+str(self.lng)+' alt:'+str(self.alt_m)+'\r\n')
            #print('MinE8 lat:',self.lat,' lng:',self.lng,' alt:',self.alt_m)            
        elif not degE9 is None:
            self.lat = Unit_Conv.degE9_to_rad(degE9.lat)
            self.lng = Unit_Conv.degE9_to_rad(degE9.lng)
            self.alt_m = degE9.alt_m
            if file is not None:
                file.write('DegE9 lat:'+str(self.lat)+' lng:'+str(self.lng)+' alt:'+str(self.alt_m)+'\r\n')
            #print('DegE9 lat:',self.lat,' lng:',self.lng,' alt:',self.alt_m)
        else:
            self.lat = None
            self.lng = None
            self.alt_m = None
class Location_ECEF:
    #WGS_84_ELLIPSOID_A = 6378137.0
    WGS_84_ELLIPSOID_B = 6356752.314245
    WGS_84_ECC_SQUARED = 0.00669437999019741354678198566736
    WGS84_E = 0.08181919092890624
    #def __init__(self):
    #    self.x = None
    #    self.y = None
    #    self.z = None

    def __init__(self, rads,file=None):
        self.easting = None
        self.northing = None
        self.up = None
        sin_lat = sin(rads.lat)
        cos_lat = cos(rads.lat)
        sin_lng = sin(rads.lng)
        cos_lng = cos(rads.lng)
        err = float(6378137.0)
        a = err + 0.303687775
        n = a / sqrt(1.0 - type(self).WGS84_E * type(self).WGS84_E * sin_lat * sin_lat)

        self.x = (n + rads.alt_m) * cos_lat * cos_lng
        self.y = (n + rads.alt_m) * cos_lat * sin_lng
        self.z = (n * (1.0 - type(self).WGS84_E * type(self).WGS84_E) + rads.alt_m) * sin_lat
        #chi = sqrt(1.0 - (type(self).WGS_84_ECC_SQUARED * sin_lat * sin_lat))
        #self.x = ((type(self).WGS_84_ELLIPSOID_A / chi) + rads.alt_m) * cos_lat * cos_lng
        #self.y = ((type(self).WGS_84_ELLIPSOID_A / chi) + rads.alt_m) * cos_lat * sin_lng
        #self.z = (type(self).WGS_84_ELLIPSOID_A * (1.0 - type(self).WGS_84_ECC_SQUARED)/chi + rads.alt_m) * sin_lat
        #print('ECEF x:',self.x,' y:',self.y,' z:',self.z)
        #if file is not None:
        #    file.write('ECEF n:'+str(n)+' a:'+str(a)+' E:'+str(type(self).WGS84_E)+' sin_lat:'+str(sin_lat)+' cos_lat:'+str(cos_lat)+' sin_lng:'+str(sin_lng)+' cos_lat:'+str(cos_lng)+' x:'+str(self.x)+' y:'+str(self.y)+' z:'+str(self.z)+'\r\n')
        

    def to_ENU(self,ref,file=None):
        xDiff = (self.x-ref.x)
        yDiff = (self.y-ref.y) 
        zDiff = (self.z-ref.z) 
        sinRefLong = sin(ref.lngRad)
        cosRefLong = cos(ref.lngRad)
        sinRefLat = sin(ref.latRad)
        cosRefLat = cos(ref.latRad)
        self.easting = -sinRefLong * xDiff + cosRefLong * yDiff
        self.northing = -sinRefLat * cosRefLong * xDiff - sinRefLat * sinRefLong * yDiff + cosRefLat * zDiff 
        self.up = cosRefLat * cosRefLong * xDiff + cosRefLat*sinRefLong*yDiff + sinRefLat+zDiff

class Location_ECEF_Ref:
    def __init__(self):
            self.x = 0.0
            self.y = 0.0
            self.z = 0.0
            self.latRad = 0.0
            self.lngRad = 0.0
            self.alt = 0.0
            self.phi = 0.0
            self.lam = 0.0
            self.sin_phi = 0.0
            self.cos_phi = 0.0
            self.cos_lam = 0.0
            self.sin_lam = 0.0
            self.easting = None
            self.northing = None
            self.up = None

    def to_ENU(self):
        self.phi = atan2(self.z, sqrt((self.x * self.x) + (self.y * self.y)))
        self.lam = atan2(self.y,self.x)
        self.sin_phi = sin(self.phi)
        self.cos_phi = cos(self.phi)
        self.sin_lam = sin(self.lam)
        self.cos_lam = cos(self.phi)
        self.easting = 0.0
        self.northing = 0.0
        self.up = 0.0

    def from_ecef(self, ecef):
        self.x = ecef.x
        self.y = ecef.y
        self.z = ecef.z

    def from_degE9(self, degE9):
        rads = Location_Rads(degE9=degE9)
        self.alt = rads.alt_m
        self.latRad = rads.lat
        self.lngRad = rads.lng
        self.from_ecef(Location_ECEF(Location_Rads(degE9=degE9)))

    def from_minE5(self, minE5):
        self.from_ecef(Location_ECEF(Location_Rads(minE5)))

    def from_minE8(self, minE8):
        self.from_ecef(Location_ECEF(Location_Rads(minE8=minE8))) 


def valid_lat_minE5(lat_minE5):
    return True if lat_minE5 <= 540000000 and lat_minE5 >= -540000000 else False

def valid_lng_minE5(lng_minE5):
    return True if lng_minE5 <= 1080000000 and lng_minE5 >= -1080000000 else False


class Unit_Conv:
    @staticmethod
    def dist(m=None, ft=None):
        if m is not None:
            return m * 3.28084
        if ft is not None:
            return ft * 0.3048

    @staticmethod
    def speed(kmh=None, mph=None):
        if kmh is not None:
            return kmh * 0.621371
        if mph is not None:
            return mph * 1.60934

    @staticmethod
    def mps_to_units(units, speed):
        if units == 'km/h':
            speed = speed * 3.6
        elif units == 'mph':
            speed = speed * 2.2369362921
        else:
            raise TypeError('unsupported sys units: ' + str(units))
        return speed

    @staticmethod
    def mps_to_mmps(mps):
        return int(0.5 + (mps * 1000.0))

    @staticmethod
    def m_to_mm(metres):
        return int(0.5 + (metres * 1000.0))

    @staticmethod
    def miles_to_m(miles):
        return (miles * 1609.344)

    @staticmethod
    def kts_to_mps(knts):
        return (knts * 0.51444444444)

    @staticmethod
    def mps2_to_mg(mps2):
        return int((mps2 * 100.019716213) + (0.5 if mps2 >= 0.0 else -0.5))

    @staticmethod
    def mps2_to_g(mps2):
        return (mps2 * 0.1019716213)

    @staticmethod
    def deg_to_degE7(degs):
        return int((degs * 10000000.0) + (0.5 if degs >= 0.0 else -0.5))

    @staticmethod
    def deg_to_minE5(degs):
        return int((degs * 6000000.0) + (0.5 if degs >= 0.0 else -0.5))

    @staticmethod
    def degE7_to_deg(degE7):
        return float(degE7) / 10000000.0

    @staticmethod
    def degE7_to_minE5(degE7):
        return int((float(degE7) * 0.6) + (0.5 if degE7 >= 0.0 else -0.5))

    @staticmethod
    def min_to_minE5(minutes):
        return int((minutes * 100000.0) + (0.5 if minutes >= 0.0 else -0.5))

    @staticmethod
    def minE5_to_min_str(minE5):
        val = abs(minE5)
        i = val // 100000
        f = val - (i * 100000)
        if minE5 < 0:
            i *= -1
        return '{}.{}'.format(i, f)

    @staticmethod
    def minE5_to_min(minE5):
        if ((minE5 == None) or (minE5 == 2147483647)):
            return float('NaN')
        else:
            return (float(minE5) / 100000.0)

    @staticmethod
    def minE5_to_deg(minE5):
        if ((minE5 == None) or (minE5 == 2147483647)):
            return float('NaN')
        else:
            return (float(minE5) / 6000000.0)

    @staticmethod
    def minE7_to_min_str(minE7):
        val = abs(minE7)
        i = val // 10000000
        f = val - (i * 10000000)
        if minE7 < 0:
            i *= -1
        return '{}.{}'.format(i, f)

    @staticmethod
    def minE8_to_min_str(minE8):
        val = abs(minE8)
        i = val // 100000000
        f = val - (i * 100000000)
        if minE8 < 0:
            i *= -1
        return '{}.{}'.format(i, f)

    @staticmethod
    def minE7_to_min(minE7):
        if ((minE7 == None) or (minE7 == 2147483647)):
            return float('NaN')
        else:
            return (float(minE7) / 10000000.0)

    @staticmethod
    def minE7_to_deg(minE7):
        if ((minE7 == None) or (minE7 == 2147483647)):
            return float('NaN')
        else:
            return (float(minE7) / 600000000.0)

    @staticmethod
    def minE5_to_rad(minE5):
        return (minE5 * (3.1415926535897932384626433832795 / 1080000000.0))

    @staticmethod
    def minE5_to_rad(minE5):
        return (minE5 * (3.1415926535897932384626433832795 / 1080000000.0))

    @staticmethod
    def minE7_to_rad(minE7):
        return (minE7 * (3.1415926535897932384626433832795 / 108000000000.0))        
    
    @staticmethod
    def minE8_to_rad(minE8):
        return (minE8 * (3.1415926535897932384626433832795 / 1080000000000.0))     

    @staticmethod
    def degE9_to_rad(degE9):
        return (degE9 * (3.1415926535897932384626433832795 / 180000000000.0))             


