# power_monitor.py

from HiwonderSDK.ros_robot_controller_sdk import Board
import time
from logger import get_logger

logger = get_logger("power_monitor")

class PowerMonitor:
    def __init__(self):
        self.board = Board()
        self.board.enable_reception()
        self.last_voltage = None
        self.last_check_time = 0
        self.cache_interval = 0.5  # seconds

    def get_voltage(self):
        now = time.time()
        if self.last_voltage is not None and now - self.last_check_time < self.cache_interval:
            return self.last_voltage  # return cached value
        raw = self.board.get_battery()
        if raw is None:
            logger.warning("⚠️ Failed to read battery voltage!")
            return None
        self.last_voltage = raw / 1000.0
        self.last_check_time = now
        return self.last_voltage

    def voltage_ok(self, threshold=6.8):
        voltage = self.get_voltage()
        if voltage is None:
            return False
        if voltage < threshold:
            logger.warning(f"🔋 LOW VOLTAGE: {voltage:.2f}V! (Threshold: {threshold:.2f}V)")
            return False
        return True