import threading
import canopen
import time
import os


class CANBackend:
    def __init__(self):
        self.network = None
        self.nodes = {}  # {1: RemoteNode(0x02), 2: RemoteNode(0x03), ...}
        self.connected = False
        self.lock = threading.Lock()
        self.polling_thread = None
        self.polling_active = False
        self.latest_data = {}
        self.eds_path = os.path.join(os.path.dirname(__file__), "eds_file", "processBoard_0.eds")

    def connect(self):
        try:
            self.network = canopen.Network()
            self.network.connect(channel='can0', bustype='socketcan')

            # PU mapping: PU1->0x02, PU2->0x03, PU3->0x04
            node_map = {
                1: 0x02,
                2: 0x04,
                3: 0x127
            }

            for pu_number, node_id in node_map.items():
                node = canopen.RemoteNode(node_id, self.eds_path)
                self.network.add_node(node)
                self.nodes[pu_number] = node

            self.connected = True
            self._start_sdo_polling()
            return True

        except Exception as e:
            print(f"[CONNECT ERROR] {e}")
            return False

    def shutdown(self):
        self.polling_active = False
        if self.network:
            self.network.disconnect()
        self.nodes.clear()
        self.connected = False

    def _start_sdo_polling(self):
        if self.polling_thread and self.polling_thread.is_alive():
            return
        self.polling_active = True
        self.polling_thread = threading.Thread(target=self._sdo_polling_loop, daemon=True)
        self.polling_thread.start()

    def _sdo_polling_loop(self):
        while self.polling_active:
            with self.lock:
                try:
                    # Poll only PU1 (node ID 0x02) for live monitor values
                    node = self.nodes.get(1)
                    if node:
                        fm1 = node.sdo[0x2004][1].raw
                        fm2 = node.sdo[0x2004][2].raw
                        fm3 = node.sdo[0x2004][3].raw
                        fm4 = node.sdo[0x2004][4].raw

                        ps1 = node.sdo[0x2005][1].raw
                        ps2 = node.sdo[0x2005][2].raw
                        ps3 = node.sdo[0x2005][3].raw
                        ps4 = node.sdo[0x2005][4].raw

                        self.latest_data["FM1"] = fm1 / 100.0
                        self.latest_data["FM2"] = fm2 / 100.0
                        self.latest_data["FM3"] = fm3 / 100.0
                        self.latest_data["FM4"] = fm4 / 100.0

                        self.latest_data["PS1"] = ps1 / 1000.0
                        self.latest_data["PS2"] = ps2 / 1000.0
                        self.latest_data["PS3"] = ps3 / 1000.0
                        self.latest_data["PS4"] = ps4 / 1000.0
                        
                        print(f"[SDO POLL] FM1: {fm1}, PS1: {ps1}")
                        print(f"[SDO POLL] FM2: {fm2}, PS2: {ps2}")
                        print(f"[SDO POLL] FM3: {fm3}, PS3: {ps3}")
                        print(f"[SDO POLL] FM4: {fm4}, PS4: {ps4}")
                except Exception as e:
                    print(f"[SDO POLL ERROR] {e}")
            time.sleep(1.0)

    def get_latest_data(self):
        with self.lock:
            return self.latest_data.copy()

    def read_current_state(self, pu_number: int):
        try:
            node = self.nodes.get(pu_number)
            if node is None:
                return "Offline"
            state_raw = node.sdo[0x2000].raw
            return self.decode_state(state_raw)
        except Exception as e:
            print(f"[PU{pu_number} READ ERROR] {e}")
            return "Offline"

    def decode_state(self, state_val: int) -> str:
        state_map = {
            0: "SYSTEM_MODE_INIT",
            1: "SYSTEM_MODE_OFF",
            2: "SYSTEM_MODE_READY",
            3: "SYSTEM_MODE_PRODUCTION",
            4: "SYSTEM_MODE_LOW_LOOP_PRESSURE",
            5: "SYSTEM_MODE_LOOP_CLEANING",
            6: "SYSTEM_MODE_HEATING_RO",
            7: "SYSTEM_MODE_RINSING_RO",
            8: "SYSTEM_MODE_HEATING_EDI",
            9: "SYSTEM_MODE_COOLING_EDI",
            10: "SYSTEM_MODE_RO_FLUSH",
            11: "SYSTEM_MODE_RO_RINSE",
            12: "SYSTEM_MODE_EDI_RINSE",
            15: "SYSTEM_MODE_FAIL_SAFE",
            16: "SYSTEM_MODE_FIRST_FLUSH",
            255: "SYSTEM_MODE_DEFAULT"
        }
        return state_map.get(state_val, f"UNKNOWN({state_val})")

    def send_state_command(self, state: str, pu_number: int, ploop_setpoint: float):
        if not self.connected:
            raise RuntimeError("CAN not connected")

        state_map = {
            "IDLE": 1,
            "PRE-PRODUCTION": 2,
            "PRODUCTION" : 3,
            "MAINTENANCE": 8,
            "EMERGENCY_STOP": 9,
            "FIRST_START": 10
        }

        if state not in state_map:
            raise ValueError(f"Invalid state: {state}")

        try:
            node = self.nodes.get(pu_number)
            if node is None:
                raise ValueError(f"PU{pu_number} not connected")

            print(f"[DEBUG] Writing state {state_map[state]} to 0x2024:{pu_number}")
            node.sdo[0x2024][pu_number].raw = state_map[state]

            print(f"[DEBUG] Writing ploop_setpoint {ploop_setpoint} to 0x2007")
            node.sdo[0x2007].raw = int(ploop_setpoint * 100)

        except Exception as e:
            print(f"[SDO WRITE ERROR] PU{pu_number}: {e}")
            raise

    def send_thermal_loop_cleaning(self, mode: str, pu_number: int):
        if not self.connected:
            raise RuntimeError("CAN not connected")

        mode_map = {
            "IDLE": 0,
            "ACTIVE": 1
        }

        if mode not in mode_map:
            raise ValueError(f"Invalid thermal loop mode: {mode}")

        try:
            node = self.nodes.get(pu_number)
            if node is None:
                raise ValueError(f"PU{pu_number} not connected")

            print(f"[DEBUG] Sending thermal loop mode {mode} to 0x2024:{pu_number}")
            node.sdo[0x2024][pu_number].raw = mode_map[mode]

        except Exception as e:
            print(f"[THERMAL LOOP ERROR] PU{pu_number}: {e}")
            raise