import serial
import struct
import time

class HILController:
    def __init__(self, emulator_port, target_port, baud=921600):
        # Port for the ESP32 acting as the Sensor
        self.emulator = serial.Serial(emulator_port, baud, timeout=0.1)
        # Port for the ESP32 running your application code
        self.target = serial.Serial(target_port, 115200, timeout=0.1)
        
    def calculate_scd30_crc(self, data):
        crc = 0xFF
        for byte in data:
            crc ^= byte
            for _ in range(8):
                if crc & 0x80:
                    crc = (crc << 1) ^ 0x31
                else:
                    crc <<= 1
                crc &= 0xFF
        return crc

    def pack_float_scd30(self, value):
        """Packs a float into the SCD30 [MSB, LSB, CRC, MSB, LSB, CRC] format"""
        b = struct.pack('>f', value)
        w1 = [b[0], b[1]]
        w2 = [b[2], b[3]]
        return w1 + [self.calculate_scd30_crc(w1)] + w2 + [self.calculate_scd30_crc(w2)]

    def set_emulator_data(self, co2, temp, hum):
        """Prepares the DATA: hex string for the Emulator ESP32"""
        # SCD30 Ready Status (0x0001 + CRC)
        ready_hex = "000103" 
        
        # Build the 18-byte measurement buffer
        payload = self.pack_float_scd30(co2) + \
                  self.pack_float_scd30(temp) + \
                  self.pack_float_scd30(hum)
        
        data_hex = "".join(f"{b:02X}" for b in payload)
        
        # We check for CMD from emulator and respond
        line = self.emulator.readline().decode('utf-8', errors='ignore').strip()
        if "CMD:0202" in line: # Master asking if data is ready
            self.emulator.write(f"DATA:{ready_hex}\n".encode())
        elif "CMD:0300" in line: # Master asking for the 18 bytes
            self.emulator.write(f"DATA:{data_hex}\n".encode())
            return True
        return False

    def read_target_output(self):
        """Reads the latest print statement from the Test Code ESP32"""
        line = self.target.readline().decode('utf-8', errors='ignore').strip()
        if "CO2:" in line:
            return line
        return None

    def close(self):
        self.emulator.close()
        self.target.close()