import threading
import canopen
import time

class CANBackend:
    def __init__(self):
        self.network = None
        self.node = None
        self.connected = False
        self.latest_data = {}
        self.lock = threading.Lock()

        # Initialize thread-related attributes
        self.polling_thread = None
        self.polling_active = False

    def connect(self, node_id, eds_path):
        try:
            self.network = canopen.Network()
            self.network.connect(bustype='socketcan', channel='can0', bitrate=250000)
            self.node = canopen.RemoteNode(node_id, eds_path)
            self.network.add_node(self.node)
            self.node.nmt.state = 'PRE-OPERATIONAL'

            self.connected = True
            self.latest_data = {}

            # Start polling thread
            self._start_sdo_polling()
            return True
        except Exception as e:
            print(f"[CAN ERROR] {e}")
            self.connected = False
            return False

    def _start_sdo_polling(self):
        if self.polling_thread and self.polling_thread.is_alive():
            return  # Already running
        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:
                    fm2 = self.node.sdo[0x2004][2].raw
                    ps1 = self.node.sdo[0x2005][1].raw
                    self.latest_data["FM2"] = fm2/100
                    self.latest_data["PS1"] = ps1/1000
                    print(f"[SDO POLL] FM2: {fm2}, PS1: {ps1}")
                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):
        # Placeholder for reading mode command
        #TODO : CODE IT
        pass
        
    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,
            "PRODUCTION": 2,
            "MAINTENANCE": 8,
            "EMERGENCY_STOP": 9,
            "FIRST_START": 10
        }

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

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

            # Write the ploop setpoint to its own index/subindex
            print(f"[DEBUG] Writing ploop_setpoint {ploop_setpoint} to index 0x2007")
            self.node.sdo[0x2007].raw = int(ploop_setpoint * 100)
        
        except Exception as e:
            print(f"[SDO WRITE ERROR] Failed to write to 0x2007:{e}")
            raise


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

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

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

        self.node.sdo[0x2024][0x01].raw = mode_map[mode]

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