from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.tools import run_task, multitask
from pybricks.tools import wait, StopWatch
from pybricks.robotics import DriveBase
from pybricks.hubs import PrimeHub
import umath
# Initialize hub and devices
hub = PrimeHub()
class BatteryDiagnostics:
    def __init__(self):
        self.voltage = 0
        self.current = 0
    def printVoltage(self):
        self.voltage = hub.battery.voltage()
        if self.voltage > 7800:
            print(f"Battery voltage is sufficient: {self.voltage}")
        elif self.voltage < 7800 :
            print(f"Charging needed: {self.voltage}")
    def printCurrent(self):
        self.current = hub.battery.current()
        print("Battery current:", self.current)
    def printAll(self):
        timeelapsed = 0
        voltageList = []
        currentList = []
        while True:
            print("\n\n\n\n\n")
            self.printVoltage()
            voltageList.append(self.voltage)
            self.printCurrent()
            currentList.append(self.current)
            wait(50)
            timeelapsed += 50
            
            if(timeelapsed >= 3000):
                break
        print("--------------FINAL RESULTS OF BATTERY DIAGNOSTICS---------------")
        print("Voltage deviation:", self.stdev(voltageList))
        print("Current deviation:", self.stdev(currentList))
    def stdev(self, vals):
        data = vals
        if len(data) < 2:
            return 0
        # Calculate the mean
        mean = sum(data) / len(data)

        # Calculate the variance (sum of squared differences from the mean, divided by n-1 for sample standard deviation)
        variance = sum([(x - mean) ** 2 for x in data]) / float(len(data) - 1)
        
        # Calculate the standard deviation (square root of the variance)
        std_dev_manual = umath.sqrt(variance)
        

        return (std_dev_manual)