# pylint: disable=E0602, E0603, W0601, W0631
#####################################################
# @module    maths_utils.py                         #
# @brief     Common maths Tools for many tasks      #
# @author    Original Code form multiple authors    #
# @date      Sept 2020                              #
# @copyright Racelogic Ltd., 2020                   #
# @Version   1.0                                    #
#####################################################
from math import cos, sin, radians, sqrt
import micropython

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)

def cosd(x):
    return cos(radians(x))

def sind(x):
    return sin(radians(x))

def lerp(x, xi, xf, ai, af):
    return ai + ((x - xi) * (af - ai) / (xf - xi))

def clerp(x, xi, xf, ai, af):
    return clamp(lerp(x, xi, xf, ai, af), ai, af)

def avg(values):
    return (sum(values)/len(values))

def stddev(values):
    val = 0
    _avg = avg(values)
    for h in values:
        val += (h-avg) **2
    return sqrt(val / len(values))

def rms(values):
    return sqrt(sum(values)/len(values))

# No advantage to these being anonymous Lambda Functions?
# 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)

class Gauss_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 __call__(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

class Reduc_Filter:
    """
    Generic digital filter object. Obtained response is defined by the reducing
    function used.

        Parameters:
            length      filter length [samples]
            fun_red     reducing function (takes a container as input and emits
                        a result)
        Optional parameters:
            init        initial value for "empty" samples
    """
    def __init__(self, length, fun_red= lambda vec: sum(vec) / len(vec), init=0):
        self.length = length
        self.fun_red = fun_red
        self.init = init
        self.start()

    def start(self):
        """Restart the filter (throw away all samples)."""
        self.container = [self.init] * self.length

    def push(self, val):
        """
        Push a new sample into filter.

            Parameters:
                val     sample to be pushed
        """
        self.container.pop()
        self.container.insert(0, val)

    def get(self):
        """
        Execute reducing function and get value.

            Output:
                filtered value
        """
        return self.fun_red(self.container)

def lower_bound(x, vec, key=lambda z: z):
    """
    Returns the index of the first element in the sorted array vec which is
    greater than or equal to x.
    """
    count = len(vec)
    first = 0
    while count > 0:
        p = first
        step = count // 2
        p += step
        if key(vec[p]) < x:
            p += 1
            first = p
            count -= step + 1
        else:
            count = step
    return p

def bytes_list_to_int(_bytes, intel=False):
    if intel: _bytes.reverse()
    result = 0
    for b in _bytes:
        result = result * 256 + int(b)
    return result

# @micropython.asm_thumb # TODO: Freezing to MPY causes error, investigate later
# def leading_zeros(r0):
#     clz(r0, r0) # return no. of leading zeros in passed 32 bit integer

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 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_hms(ms):
    hms = 3 * [None]
    hms[0] = ms // 3600000
    ms -= (hms[0] * 3600000)
    hms[1] = ms // 60000
    ms -= (hms[1] * 60000)
    hms[2] = ms / 1000.0
    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 str_num_dec_places(string):
    arr = string.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
    elif dec_places == 3:
        value += 0.0005
        value = floor(value * 1000)/1000
    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):
    #ignores values after the 6th decimal value
    arr = '{:f}'.format(value - int(value)).split('.')
    decstr = arr[1].replace('0', '')
    return bool(decstr)

def fmt_dec_places_float(dec_places, value):
    scale = pow(10, dec_places)
    value *= scale
    value += -0.5 if (value < 0) else 0.5
    ivalue = int(value)
    string = '{:0>{dp}d}'.format(ivalue, dp=dec_places+1)
    i = dec_places * -1
    if (not 0 == dec_places):
        return string[:i] + '.' + string[i:]
    else:
        return string

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

def norm_fmt_dec_places(dec_places, value):
    return fmt_dec_places(dec_places, normalise_value(value, dec_places))

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


#end