import time
import asyncio
import json

from lib.topic_message import TopicMessage, TMQueue
from lib.TMP117 import TMP117

async def measure_sensors(
        sensors: list[TMP117], 
        interval_s: float | int, 
        microcontroller_id: int,
        message_queue: TMQueue,
        temperature_topic: str):
    """
    Readout configuration of all sensors, then timestamp, temperature of all sensors. Adds data to message_queue. Performs every interval_s, correcting for execution time.
    """
    next_time = time.ticks_us()
    while True:
        # Initialise
        temperatures = []
        timestamps = []
        configs = []
    
    
        # Readout configuration of sensors pre-temperature measurement
        for sensor in sensors:
            try:
                configs.append(sensor.read_configuration_integer())
            except OSError as E:
                print(f"ERROR: Sensor {sensor.name}, {E}")
                configs.append(None)
    
        # Read time and temperature
        for sensor in sensors:
            try:
                temperatures.append(sensor.read_temperature())
            except OSError as E:
                print(f"ERROR: Sensor {sensor.name}, {E}")
                temperatures.append(None)
            timestamps.append(time.time_ns())
    
        # Package into message
        for i in range(len(sensors)):
            message = json.dumps({
                "micro": microcontroller_id,
                "sensor": sensors[i].name,
                "temperature": temperatures[i],
                "time": timestamps[i],
                "config": configs[i]
            })
            x = TopicMessage(temperature_topic, message)
            message_queue.add(x)

        # Calculate time to sleep
        next_time = time.ticks_add(next_time, int(interval_s*1000000))
        sleep_time = time.ticks_diff(next_time, time.ticks_us())/1e6
        await asyncio.sleep(sleep_time)

async def switch_inbound_message_queue(inbound_queue: TMQueue, interval_s):
    """Checks inbound queue and decides what to do with the messages"""
    while True:
        # Iterate through all inbound messages and execute relevant task.
        while not inbound_queue.is_empty():
            tmsg = inbound_queue.get()
            topic = tmsg.topic
            message = tmsg.message
            pass
        await asyncio.sleep(interval_s)