import time, system, gc
import rgb, consts, default_icons
n_measurements = 20

if int(consts.INFO_FIRMWARE_BUILD) < 21:
    rgb.scrolltext('Newer firmware needed')
    time.sleep(8)
    system.reboot()

import mpu6050

if not mpu6050.has_sensor():
    rgb.scrolltext('Addon required')
    time.sleep(6)
    system.reboot()

mpu6050.init()

def noisy_average(measurements):
    sorted_measurements = sorted(measurements)

    # Lol, such hack. So wow.
    if len(measurements) < 5:
        return 0.0

    total = 0
    for i in range(2, len(sorted_measurements) - 2):
        total += sorted_measurements[i]
    return total / (len(sorted_measurements)-4)

data, size, frames = default_icons.animation_loading
rgb.gif(data, (13, 1), size, frames)

while True:
    measurements = []
    for i in range(0, n_measurements):
        try:
            measurement = mpu6050.get_temp()
            if measurement == 36.53:
                mpu6050.init()
                raise OSError('Probably bad result')
            print(measurement)
        except:
            continue
        print(measurement)
        measurements.append(measurement)
        time.sleep(0.1)
    average = noisy_average(measurements)
    # print(average)

    rgb.clear()
    rgb.text('%.1f' % average, pos=(0, 1))
    rgb.text('C', pos=(26, 1))
    gc.collect()