import threading
import canopen
import time
import os


class CANBackend:
    def __init__(self, eds_file =None):
        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 = {
            1: {},  # PU1
            2: {},  # PU2
            3: {}   # PU3
        }
        if eds_file is None:
            self.eds_path = os.path.join(os.path.dirname(__file__), "eds_file", "processBoard_0.eds")
        else:
            self.eds_path = eds_file

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

            # PU mapping: PU1->0x02, PU2->0x04, PU3->0x127
            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:
                    for pu_number, node in self.nodes.items():
                        try:
                            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

                            mv02Cmd = node.sdo[0x2014][1].raw
                            mv03Cmd = node.sdo[0x2012][1].raw
                            mv04Cmd = node.sdo[0x2019][1].raw
                            mv05Cmd = node.sdo[0x2020][1].raw
                            mv06Cmd = node.sdo[0x2021][1].raw
                            mv07Cmd = node.sdo[0x2015][1].raw
                            mv08Cmd = node.sdo[0x2022][1].raw

                            # mv02fb = node.sdo[0x3000][2].raw
                            # mv03fb = node.sdo[0x3000][3].raw
                            # mv04fb = node.sdo[0x3000][4].raw
                            # mv05fb = node.sdo[0x3000][5].raw
                            # mv06fb = node.sdo[0x3000][6].raw
                            # mv07fb = node.sdo[0x3000][7].raw
                            # mv08fb = node.sdo[0x3000][8].raw

                            self.latest_data[pu_number] = {
                                "FM1": (fm1 / 100.0) * 60.0,
                                "FM2": (fm2 / 100.0) * 60.0,
                                "FM3": (fm3 / 100.0) * 60.0,
                                "FM4": (fm4 / 100.0) * 60.0,

                                "PS1": ps1 / 1000.0,
                                "PS2": ps2 / 1000.0,
                                "PS3": ps3 / 1000.0,
                                "PS4": ps4 / 1000.0,

                                "MV02_sp" : mv02Cmd / 100.0,
                                "MV03_sp" : mv03Cmd / 100.0,
                                "MV04_sp" : mv04Cmd / 100.0,
                                "MV05_sp" : mv05Cmd / 100.0,
                                "MV06_sp" : mv06Cmd / 100.0,
                                "MV07_sp" : mv07Cmd / 100.0,
                                "MV08_sp" : mv08Cmd / 100.0,

                                # "MV02" : mv02fb,
                                # "MV03" : mv03fb,
                                # "MV04" : mv04fb,
                                # "MV05" : mv05fb,
                                # "MV06" : mv06fb,
                                # "MV07" : mv07fb,
                                # "MV08" : mv08fb,
                            }

                            print(f"[PU{pu_number}] FM1: {fm1}, PS1: {ps1}")
                        except Exception as inner_e:
                            print(f"[SDO READ ERROR] PU{pu_number}: {inner_e}")
                except Exception as outer_e:
                    print(f"[SDO POLL ERROR] {outer_e}")

            time.sleep(1.0)

    def get_latest_data(self, pu_number: int):
        with self.lock:
            return self.latest_data.get(pu_number, {}).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][0x01].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