##
# @module    util
# @brief     Utility functions and classes
# @version   1.0
#

from math import cos, sin, sqrt, radians

class Filter:
    def __init__(self):
        # 2nd order IIR Gauss LP filter, 2.1 Hz BW, 3 Hz wc, fs = 60 Hz
        self.a = (1.0000000000, -1.4950558121,  0.5668457092)
        self.b = (0.0179474742,  0.0358949485,  0.0179474742)
        # Filter state
        self.w = [0, 0]

    def update(self, x):
        # Biquad transposed direct form 2
        w = x - self.a[1] * self.w[0] - self.a[2] * self.w[1]
        y = self.b[0] * w + self.b[1] * self.w[0] + self.b[2] * self.w[1]
        self.w[1] = self.w[0]
        self.w[0] = w
        return y

    def ss(self, x):
        # Closed form for steady-state condition
        self.w[0] = self.w[1] = x / (1 + self.a[1] + self.a[2])

def clamp(x, a, b):
    m0 = min((a, b))
    m1 = max((a, b))
    return m0 if x < m0 else (m1 if x > m1 else x)

cosd = lambda x: cos(radians(x))

sind = lambda x: sin(radians(x))

lerp = lambda x, xi, xf, ai, af: ai + ((x - xi) * (af - ai) / (xf - xi))

clerp = lambda x, xi, xf, ai, af: clamp(lerp(x, xi, xf, ai, af), ai, af)

def get_date_time():
    from vts import clock_get
    time = [None] * 3
    date = [None] * 3
    rtc = clock_get()
    date[0] = rtc['day of month']
    date[1] = rtc['month']
    date[2] = rtc['year']
    time[0] = rtc['hours']
    time[1] = rtc['minutes']
    time[2] = rtc['seconds']
    return date, time

def file_list(directory, extension):
    from os import listdir
    try:
        ext = extension.upper()
        list = [f for f in listdir(directory) if f[-4:].upper()==ext]
    except:
        list = []
    return list

def check_directory_valid(path):
    from os import stat, mkdir
    try:
        stats = stat(path)
        ifmt = stats[0] & 0xF000
    except Exception as e:
        #print (e)
        ifmt = None
    if ifmt is None:
        mkdir(path)
    else:
        if not ifmt == 0x4000:
            raise Exception('object exists at {}'.format(path))

def check_file_exists(path):
    from os import stat
    try:
        stats = stat(path)
        #print ('stats {}'.format(stats))
        if (not stats[0] & 0x8000):
            raise Exception('not a file')
        return True
    except Exception as e:
        return False

def tod_to_hmsm(ms):
    hmsm = 4 * [None]
    hmsm[0] = ms // 3600000
    ms -= (hmsm[0] * 3600000)
    hmsm[1] = ms // 60000
    ms -= (hmsm[1] * 60000)
    hmsm[2] = ms // 1000
    ms -= (hmsm[2] * 1000)
    hmsm[3] = ms
    return hmsm

def tod_to_msm(ms):
    hmsm = 3 * [None]
    hmsm[0] = ms // 60000
    ms -= (hmsm[0] * 60000)
    hmsm[1] = ms // 1000
    ms -= (hmsm[1] * 1000)
    hmsm[2] = ms
    return hmsm

def tod_to_hms(ms):
    hms = 3 * [None]
    if (ms < 0):
        neg = True
        ms *= -1
    else:
        neg = False
    hms[0] = ms // 3600000
    ms -= (hms[0] * 3600000)
    hms[1] = ms // 60000
    ms -= (hms[1] * 60000)
    hms[2] = ms / 1000.0
    if (neg):
        hms[0] *= -1
    return hms

def tod_to_minsecs(ms):
    minsec = 2 * [None]
    minsec[0] = ms // 60000
    ms -= (minsec[0] * 60000)
    minsec[1] = ms / 1000
    return minsec

def tstr_to_ms(tstr):
    # Converts string of format +hh:mm to milliseconds
    lst = tstr.split(':')
    return (60 * 1000 * ((60 * int(lst[0])) + (int(lst[1]))))

def str_num_dec_places(str):
    arr = str.split('.')
    if len(arr) < 2:
        cnt = 0
    else:
        decstr = arr[1]
        if len(decstr) == 0:
            cnt = 0
        else:
            while decstr[-1] == '0' and len(decstr) > 1:
                decstr = decstr[:-1]
            cnt = len(decstr)
    return cnt

def normalise_value(value, dec_places):
    from math import floor
    if dec_places == 0:
        value += 0.5
        value = floor(value)
    elif dec_places == 1:
        value += 0.05
        value = floor(value * 10)/10
    elif dec_places == 2:
        value += 0.005
        value = floor(value * 100)/100
    else:
        raise Exception('Number dec places ')
    return value

def num_dec_places(value):
    if isinstance(value, int):
        cnt = 0
    elif isinstance(float, value):
        cnt = str_num_dec_places('{:f}'.format(value - int(value)))
    else:
        raise Exception('Invalid value type')
    return cnt

def has_fraction(value):
    arr = '{:f}'.format(value - int(value)).split('.')
    decstr = arr[1].replace('0', '')
    if decstr == '':
        return False
    else:
        return True

def fmt_dec_places_float(value, dp, ip):
    scale = pow(10, dp)
    neg = True if (value < 0.0) else False
    value = abs(value)
    value *= scale
    value += 0.5
    ivalue = int(value)
    if (neg):
        str = '-{:0>{len}d}'.format(ivalue, len=dp+ip)
    else:
        str = '{:0>{len}d}'.format(ivalue, len=dp+ip)
    i = dp * -1
    if (not 0 == dp):
        return str[:i] + '.' + str[i:]
    else:
        return str

def fmt_dec_places(dec_places, value, fix):
    if isinstance(value, int):
        str = '{:d}'.format(value)
    elif ((not has_fraction(value)) and (not fix)):
        str = '{:d}'.format(int(value))
    else:
        str = fmt_dec_places_float(dec_places, value)
    return str

def norm_fmt_dec_places(value, dp, ip=1):
    return fmt_dec_places_float(normalise_value(value, dp), dp, ip)

def inv_str(fmt):
    return fmt.format(0).replace('0','-')

class Ring_Buffer:
    def __init__(self, size, val=False):
        if size <= 0:
            raise ValueError('size must be positive')
        self.pos = 0
        self.container = [val] * size
        self.size = size
        self.sum = sum(self.container)
        if size & (size - 1) == 0:
            self.update_pos = self._update_pos_powerof2
        else:
            self.update_pos = self._update_pos_generic

    def _update_pos_powerof2(self, val):
        return (val + 1) & (self.size - 1)

    def _update_pos_generic(self, val):
        return (val + 1) % self.size

    def push(self, value):
        self.sum -= self.container[self.pos]
        self.container[self.pos] = value
        self.sum += value
        self.pos = self.update_pos(self.pos)

    def get_sum(self):
        return self.sum

    def get_size(self):
        return self.size

    def get_avg(self):
        return self.sum / self.size

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_Rads:
    def __init__(self, minE5=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
        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

    def __init__(self):
        self.x = None
        self.y = None
        self.z = None

    def from_rads(self, rads):
        sin_lat = sin(rads.lat)
        cos_lat = cos(rads.lat)
        sin_lng = sin(rads.lng)
        cos_lng = cos(rads.lng)
        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

class Location_ECEF_Ref:
    def __init__(self):
            self.x = None
            self.y = None
            self.z = None
            self.phi = None
            self.sin_phi = None
            self.cos_phi = None

    def from_ecef(self, ecef):
        self.x = ecef.x
        self.y = ecef.y
        self.z = ecef.z
        self.phi = atan2(self.z, sqrt((self.x * self.x) + (self.y * self.y)))
        self.sin_phi = sin(self.phi)
        self.cos_phi = cod(self.phi)

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

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 min_to_minE5(min):
        return int((min * 100000.0) + (0.5 if min >= 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 '{}.{:05d}'.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 minE5_to_rad(minE5):
        return (minE5 * (3.1415926535897932384626433832795 / 1080000000.0))
