#!/usr/bin/env python
"""
CAN Bus IoT Library for Raspberry Pi
Support for digital and analog vertebrae communication via CAN bus

Usage:
    pi@raspberrypi:~/codis/canbus $ sudo ip link set can0 type can bitrate 100000
    pi@raspberrypi:~/codis/canbus $ sudo ifconfig can0 up
    pi@raspberrypi:~/codis/canbus $ sudo ifconfig can0 down

Reference: https://github.com/hardbyte/python-can/blob/main/examples/asyncio_demo.py
"""

import os
import can
from datetime import datetime
from time import sleep
from typing import Optional, Union
import RPi.GPIO as GPIO
import atexit

# =============================================================================
# CONSTANTS
# =============================================================================
CAN_CHANNEL = 'can0'
CAN_INTERFACE = 'socketcan'
CAN_BITRATE = 100000

USE_LEDS = False
VERBOSE = False
LED_W = 12
LED_B = 13
TIMEOUT_RCV = 0.5

# Arbitration IDs
ARB_ID_ANALOG_BASE = 16
ARB_ID_ANALOG_VERSION = 512
ARB_ID_AOUT_BASE = 784
ARB_ID_DIGITAL_BASE = 800

# Voltage conversion constants
VOLTAGE_MIN = -10.0
VOLTAGE_MAX = 10.0
AIN_MAX_VALUE = 26624
AOUT_MAX_VALUE = 4095
AOUT_VOLTAGE_MAX = 10.0

# =============================================================================
# GLOBAL BUS INSTANCE (SINGLETON)
# =============================================================================
_bus_instance: Optional[can.interface.Bus] = None


def get_bus() -> can.interface.Bus:
    """
    Obté la instància global del bus CAN (singleton pattern).
    Crea la instància si no existeix.
    
    Returns:
        Instància del bus CAN
    """
    global _bus_instance
    if _bus_instance is None:
        try:
            _bus_instance = can.interface.Bus(
                channel=CAN_CHANNEL,
                interface=CAN_INTERFACE
            )
            if VERBOSE:
                print(f"CAN bus initialized on {CAN_CHANNEL}")
        except can.CanError as e:
            print(f"Error initializing CAN bus: {e}")
            raise
    return _bus_instance


def close_bus() -> None:
    """
    Tanca el bus CAN global i allibera els recursos.
    """
    global _bus_instance
    if _bus_instance is not None:
        try:
            _bus_instance.shutdown()
            if VERBOSE:
                print("CAN bus closed properly")
        except Exception as e:
            print(f"Error closing CAN bus: {e}")
        finally:
            _bus_instance = None


# Registrar el tancament automàtic quan el programa acabi
atexit.register(close_bus)


# =============================================================================
# GPIO SETUP (LED INDICATORS)
# =============================================================================
if USE_LEDS:
    GPIO.setmode(GPIO.BCM)
    GPIO.setwarnings(False)
    GPIO.setup(LED_W, GPIO.OUT)
    GPIO.setup(LED_B, GPIO.OUT)


# =============================================================================
# CONFIGURATION DECODERS
# =============================================================================
def dvert_cfg_from_number(n: int) -> str:
    """
    Converteix el número de configuració de vertebra digital a string descriptiu.
    
    Args:
        n: Número de configuració
        
    Returns:
        String amb la configuració (ex: "A:din, B:dout")
    """
    config_map = {
        17: "A:din, B:din",
        33: "A:din, B:dout",
        18: "A:dout, B:din",
        34: "A:dout, B:dout",
        20: "A:pwm, B:din",
        36: "A:pwm, B:dout",
        65: "A:din, B:pwm",
        66: "A:dout, B:pwm",
        130: "A:dout, B:touch",
        129: "A:din, B:touch",
        132: "A:pwm, B:touch"
    }
    return config_map.get(n, "A:?, B:?")


def avert_cfg_from_number(by_rib_a: int, by_rib_b: int) -> str:
    """
    Converteix els bytes de configuració de vertebra analògica a string descriptiu.
    
    Args:
        by_rib_a: Byte de configuració del rib A
        by_rib_b: Byte de configuració del rib B
        
    Returns:
        String amb la configuració (ex: "A:ain, B:aout")
    """
    def get_mode(by_rib: int) -> str:
        if by_rib == 1:
            return "ain"
        elif by_rib == 2:
            return "aout"
        else:
            return "?"
    
    return f"A:{get_mode(by_rib_a)}, B:{get_mode(by_rib_b)}"


# =============================================================================
# VOLTAGE CONVERSION UTILITIES
# =============================================================================
def ain2v(ain_value: int) -> float:
    """
    Converteix valor analògic d'entrada (0-26624) a voltatge (-10V a +10V).
    
    Args:
        ain_value: Valor digital llegit (0-26624)
        
    Returns:
        Voltatge en volts amb 2 decimals
    """
    return round((((20 * float(ain_value)) / AIN_MAX_VALUE) + VOLTAGE_MIN) * 100) / 100


def v2aout(voltage_0_10: float) -> int:
    """
    Converteix voltatge (0-10V) a valor digital per sortida analògica.
    
    Args:
        voltage_0_10: Voltatge entre 0 i 10V
        
    Returns:
        Valor digital entre 0 i 4095
    """
    ret_v = round((voltage_0_10 * AOUT_MAX_VALUE) / AOUT_VOLTAGE_MAX)
    return max(0, min(AOUT_MAX_VALUE, ret_v))


# =============================================================================
# CAN MESSAGE BUILDERS
# =============================================================================
def ain_msg(addr: str, side: str) -> can.Message:
    """
    Crea missatge CAN per llegir entrada analògica.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        
    Returns:
        Missatge CAN configurat
    """
    i2c_addr = int(addr, 2)
    offset = 0 if side.lower() == 'a' else 256
    
    return can.Message(
        arbitration_id=ARB_ID_ANALOG_BASE + i2c_addr + offset,
        data=[],
        is_extended_id=False,
        is_remote_frame=True,
        dlc=0,
        timestamp=datetime.timestamp(datetime.now())
    )


def aout_msg(addr: str, side: str, ndac: int, value: int) -> can.Message:
    """
    Crea missatge CAN per escriure sortida analògica.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        ndac: Número de DAC (1, 2, 3 o 4)
        value: Valor entre 0 i 4095 (0V a 10V)
        
    Returns:
        Missatge CAN configurat
    """
    i2c_addr = int(addr, 2)
    
    # Calcular el byte de comandament segons el side i ndac
    base_cmd = 2 if side.lower() == 'a' else 18
    ndac_offset = {1: 0, 2: 32, 3: 64, 4: 96}.get(ndac, 0)
    
    msg_data = [
        base_cmd + ndac_offset,
        value // 256,
        value % 256
    ]
    
    if VERBOSE:
        print(f"AOUT message: {msg_data}")
    
    return can.Message(
        arbitration_id=ARB_ID_AOUT_BASE + i2c_addr,
        data=msg_data,
        is_extended_id=False,
        is_remote_frame=False,
        dlc=3,
        timestamp=datetime.timestamp(datetime.now())
    )


def dout_msg(addr: str, side: str, value: int) -> can.Message:
    """
    Crea missatge CAN per escriure sortida digital (byte complet).
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        value: Valor del byte (0-255)
        
    Returns:
        Missatge CAN configurat
    """
    i2c_addr = int(addr, 2)
    msg_data = [2 if side.lower() == 'a' else 18, value]
    
    if VERBOSE:
        print(f"DOUT message: {msg_data}")
    
    return can.Message(
        arbitration_id=ARB_ID_DIGITAL_BASE + i2c_addr,
        data=msg_data,
        is_extended_id=False,
        is_remote_frame=False,
        dlc=2,
        timestamp=datetime.timestamp(datetime.now())
    )


def doutbit_msg(addr: str, side: str, posbyte: int, value: int) -> can.Message:
    """
    Crea missatge CAN per escriure un bit específic de sortida digital.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        posbyte: Posició del bit (0-7)
        value: Valor del bit (0 o 1)
        
    Returns:
        Missatge CAN configurat
    """
    i2c_addr = int(addr, 2)
    base_val = 10 if side.lower() == 'a' else 26
    val = base_val + 32 * posbyte
    msg_data = [val, value]
    
    if VERBOSE:
        print(f"DOUTBIT message: {msg_data}")
    
    return can.Message(
        arbitration_id=ARB_ID_DIGITAL_BASE + i2c_addr,
        data=msg_data,
        is_extended_id=False,
        is_remote_frame=False,
        dlc=2,
        timestamp=datetime.timestamp(datetime.now())
    )


def din_msg(addr: str) -> can.Message:
    """
    Crea missatge CAN per llegir entrada digital.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        
    Returns:
        Missatge CAN configurat
    """
    i2c_addr = int(addr, 2)
    
    return can.Message(
        arbitration_id=ARB_ID_DIGITAL_BASE + i2c_addr,
        data=[],
        is_extended_id=False,
        is_remote_frame=True,
        dlc=0,
        timestamp=datetime.timestamp(datetime.now())
    )


def dversion_msg(addr: str) -> can.Message:
    """
    Crea missatge CAN per llegir versió de vertebra digital.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        
    Returns:
        Missatge CAN configurat
    """
    i2c_addr = int(addr, 2)
    
    return can.Message(
        arbitration_id=ARB_ID_DIGITAL_BASE + i2c_addr,
        data=[],
        is_extended_id=False,
        is_remote_frame=True,
        dlc=0,
        timestamp=datetime.timestamp(datetime.now())
    )


def aversion_msg(addr: str) -> can.Message:
    """
    Crea missatge CAN per llegir versió de vertebra analògica.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        
    Returns:
        Missatge CAN configurat
    """
    i2c_addr = int(addr, 2)
    
    return can.Message(
        arbitration_id=ARB_ID_ANALOG_BASE + i2c_addr + ARB_ID_ANALOG_VERSION,
        data=[],
        is_extended_id=False,
        is_remote_frame=True,
        dlc=0,
        timestamp=datetime.timestamp(datetime.now())
    )


# =============================================================================
# LOW-LEVEL CAN SEND/RECEIVE
# =============================================================================
def send_can(bus: can.interface.Bus, msg: can.Message) -> None:
    """
    Envia un missatge pel bus CAN amb indicador LED opcional.
    
    Args:
        bus: Instància del bus CAN
        msg: Missatge a enviar
    """
    if USE_LEDS:
        GPIO.output(LED_W, GPIO.HIGH)
    
    try:
        bus.send(msg)
        sleep(0.001)  # Evitar llegir el que s'acaba de trametre
    except can.CanError as e:
        if VERBOSE:
            print(f"Error sending CAN message: {e}")
        raise
    finally:
        if USE_LEDS:
            GPIO.output(LED_W, GPIO.LOW)


def recv_can(bus: can.interface.Bus) -> Optional[can.Message]:
    """
    Rep un missatge del bus CAN amb timeout i indicador LED opcional.
    
    Args:
        bus: Instància del bus CAN
        
    Returns:
        Missatge rebut o None si timeout
    """
    if USE_LEDS:
        GPIO.output(LED_B, GPIO.HIGH)
    
    try:
        msg = bus.recv(TIMEOUT_RCV)
        return msg
    except can.CanError as e:
        if VERBOSE:
            print(f"Error receiving CAN message: {e}")
        return None
    finally:
        if USE_LEDS:
            GPIO.output(LED_B, GPIO.LOW)


# =============================================================================
# HIGH-LEVEL API FUNCTIONS
# =============================================================================
def ain(addr: str, side: str, ndac: int) -> Union[int, str]:
    """
    Llegeix entrada analògica.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        ndac: Número de DAC a llegir (1, 2, 3 o 4)
        
    Returns:
        Valor digital llegit (0-26624) o "Error" si falla
    """
    try:
        bus = get_bus()
        send_can(bus, ain_msg(addr, side))
        msg = recv_can(bus)
        
        if VERBOSE and msg:
            print(f"AIN response: {msg}")
            print(f"  is_remote_frame: {msg.is_remote_frame}")
            print(f"  data: {msg.data}")
            print(f"  dlc: {msg.dlc}")
        
        if msg is None:
            if VERBOSE:
                print(f"No CAN answer in {TIMEOUT_RCV} seconds")
            return "Error"
        
        pos = ndac * 2 - 2
        if pos + 1 >= len(msg.data):
            if VERBOSE:
                print(f"Insufficient data in message for DAC {ndac}")
            return "Error"
        
        return 256 * msg.data[pos] + msg.data[pos + 1]
        
    except Exception as e:
        if VERBOSE:
            print(f"Error in ain(): {e}")
        return "Error"


def din(addr: str, side: str) -> Union[str, str]:
    """
    Llegeix entrada digital (8 bits).
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        
    Returns:
        String de 8 bits (ex: '10101010') o "Error" si falla
    """
    try:
        bus = get_bus()
        send_can(bus, din_msg(addr))
        msg = recv_can(bus)
        
        if VERBOSE and msg:
            print(f"DIN response: {msg}")
            print(f"  is_remote_frame: {msg.is_remote_frame}")
            print(f"  data: {msg.data}")
            print(f"  dlc: {msg.dlc}")
        
        if msg is None:
            if VERBOSE:
                print(f"No CAN answer in {TIMEOUT_RCV} seconds")
            return "Error"
        
        pos = 3 if side.lower() == 'a' else 4
        if pos >= len(msg.data):
            if VERBOSE:
                print(f"Insufficient data in message")
            return "Error"
        
        valor = msg.data[pos]
        # Invertir els bits i formatejar com a string binari de 8 bits
        return format(~valor & 255, 'b').zfill(8)
        
    except Exception as e:
        if VERBOSE:
            print(f"Error in din(): {e}")
        return "Error"


def aout(addr: str, side: str, ndac: int, value: int) -> None:
    """
    Escriu sortida analògica.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        ndac: Número de DAC (1, 2, 3 o 4)
        value: Valor entre 0 i 4095 (0V a 10V)
    """
    try:
        bus = get_bus()
        send_can(bus, aout_msg(addr, side, ndac, value))
    except Exception as e:
        if VERBOSE:
            print(f"Error in aout(): {e}")


def dout(addr: str, side: str, value: int) -> None:
    """
    Escriu sortida digital (byte complet).
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        value: Valor del byte (0-255)
    """
    try:
        bus = get_bus()
        send_can(bus, dout_msg(addr, side, value))
    except Exception as e:
        if VERBOSE:
            print(f"Error in dout(): {e}")


def doutbit(addr: str, side: str, posbyte: int, value: int) -> None:
    """
    Escriu un bit específic de sortida digital.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        side: 'A' o 'B'
        posbyte: Posició del bit (0-7)
        value: Valor del bit (0 o 1)
    """
    try:
        bus = get_bus()
        send_can(bus, doutbit_msg(addr, side, posbyte, value))
    except Exception as e:
        if VERBOSE:
            print(f"Error in doutbit(): {e}")


def dsetup(addr: str, mode_a: str, mode_b: str) -> None:
    """
    Configura els modes de treball d'una vertebra digital.
    
    Restriccions:
    - PWM: Només un rib (A o B) pot gestionar PWM
    - Touch: Només el rib B pot gestionar 8 entrades tàctils
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        mode_a: Mode del rib A ('din', 'dout', 'pwm')
        mode_b: Mode del rib B ('din', 'dout', 'pwm', 'touch')
    """
    try:
        bus = get_bus()
        i2c_addr = int(addr, 2)
        
        # Validació: no es pot tenir PWM als dos costats
        if mode_a.lower() == 'pwm' and mode_b.lower() == 'pwm':
            print("Error: PWM només pot estar a un costat (A o B)")
            return
        
        # Validació: touch només pot estar a B
        if mode_a.lower() == 'touch':
            print("Error: Touch només s'accepta al costat B")
            return
        
        # Configurar direccions dels ports
        d1 = 0 if mode_a.lower() in ['dout', 'pwm'] else 255
        d2 = 0 if mode_b.lower() in ['dout', 'pwm'] else 255
        
        msg_data = [0, d1, d2]
        if VERBOSE:
            print(f"Setup message: {msg_data}")
        
        can_msg = can.Message(
            arbitration_id=ARB_ID_DIGITAL_BASE + i2c_addr,
            data=msg_data,
            is_extended_id=False,
            is_remote_frame=False,
            dlc=3,
            timestamp=datetime.timestamp(datetime.now())
        )
        send_can(bus, can_msg)
        
        # Configurar PWM si cal
        if mode_a.lower() == 'pwm':
            msg_data = [4, 0]
            can_msg = can.Message(
                arbitration_id=ARB_ID_DIGITAL_BASE + i2c_addr,
                data=msg_data,
                is_extended_id=False,
                is_remote_frame=False,
                dlc=2,
                timestamp=datetime.timestamp(datetime.now())
            )
            send_can(bus, can_msg)
        elif mode_b.lower() == 'pwm':
            msg_data = [4, 1]
            can_msg = can.Message(
                arbitration_id=ARB_ID_DIGITAL_BASE + i2c_addr,
                data=msg_data,
                is_extended_id=False,
                is_remote_frame=False,
                dlc=2,
                timestamp=datetime.timestamp(datetime.now())
            )
            send_can(bus, can_msg)
        
        # Configurar Touch si cal
        if mode_b.lower() == 'touch':
            msg_data = [7, 1]
            can_msg = can.Message(
                arbitration_id=ARB_ID_DIGITAL_BASE + i2c_addr,
                data=msg_data,
                is_extended_id=False,
                is_remote_frame=False,
                dlc=2,
                timestamp=datetime.timestamp(datetime.now())
            )
            send_can(bus, can_msg)
        
        if VERBOSE:
            print(f"Configuration set: A={mode_a}, B={mode_b}")
            
    except Exception as e:
        if VERBOSE:
            print(f"Error in dsetup(): {e}")


def dversion(addr: str) -> str:
    """
    Llegeix la versió del firmware d'una vertebra digital.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        
    Returns:
        String amb la versió (ex: '1.5') o '0.0' si error
    """
    try:
        bus = get_bus()
        send_can(bus, dversion_msg(addr))
        msg = recv_can(bus)
        
        if VERBOSE and msg:
            print(f"DVERSION response: {msg}")
        
        if msg is not None and len(msg.data) >= 3:
            version = f"{msg.data[1]}.{msg.data[2]}"
            if VERBOSE:
                cfg = dvert_cfg_from_number(msg.data[0])
                print(f"Digital vert {addr} --> Cfg: {cfg}. Version: {version}")
            return version
        else:
            if VERBOSE:
                print(f"No CAN answer in {TIMEOUT_RCV} seconds")
            return "0.0"
            
    except Exception as e:
        if VERBOSE:
            print(f"Error in dversion(): {e}")
        return "0.0"


def getdsetup(addr: str) -> str:
    """
    Llegeix la configuració actual d'una vertebra digital.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        
    Returns:
        String amb la configuració (ex: 'A:din, B:dout') o 'A:?, B:?' si error
    """
    try:
        bus = get_bus()
        send_can(bus, dversion_msg(addr))
        msg = recv_can(bus)
        
        if VERBOSE and msg:
            print(f"GETDSETUP response: {msg}")
        
        if msg is not None and len(msg.data) >= 3:
            dsetup = dvert_cfg_from_number(msg.data[0])
            version = f"{msg.data[1]}.{msg.data[2]}"
            if VERBOSE:
                print(f"Digital vert {addr} --> Cfg: {dsetup}. Version: {version}")
            return dsetup
        else:
            if VERBOSE:
                print(f"No CAN answer in {TIMEOUT_RCV} seconds")
            return "A:?, B:?"
            
    except Exception as e:
        if VERBOSE:
            print(f"Error in getdsetup(): {e}")
        return "A:?, B:?"


def aversion(addr: str) -> str:
    """
    Llegeix la versió del firmware d'una vertebra analògica.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        
    Returns:
        String amb la versió (ex: '1.4') o '0.0' si error
    """
    try:
        bus = get_bus()
        send_can(bus, aversion_msg(addr))
        msg = recv_can(bus)
        
        if VERBOSE and msg:
            print(f"AVERSION response: {msg}")
        
        if msg is not None and len(msg.data) >= 4:
            version = f"{msg.data[1]}.{msg.data[2]}"
            if VERBOSE:
                cfg = avert_cfg_from_number(msg.data[0], msg.data[3])
                print(f"Analog vert {addr} --> Cfg: {cfg}. Version: {version}")
            return version
        else:
            if VERBOSE:
                print(f"No CAN answer in {TIMEOUT_RCV} seconds")
            return "0.0"
            
    except Exception as e:
        if VERBOSE:
            print(f"Error in aversion(): {e}")
        return "0.0"


def getasetup(addr: str) -> str:
    """
    Llegeix la configuració actual d'una vertebra analògica.
    
    Args:
        addr: Adreça en format binari (ex: '0000')
        
    Returns:
        String amb la configuració (ex: 'A:ain, B:aout') o 'A:?, B:?' si error
    """
    try:
        bus = get_bus()
        send_can(bus, aversion_msg(addr))
        msg = recv_can(bus)
        
        if VERBOSE and msg:
            print(f"GETASETUP response: {msg}")
        
        if msg is not None and len(msg.data) >= 4:
            asetup = avert_cfg_from_number(msg.data[0], msg.data[3])
            version = f"{msg.data[1]}.{msg.data[2]}"
            if VERBOSE:
                print(f"Analog vert {addr} --> Cfg: {asetup}. Version: {version}")
            return asetup
        else:
            if VERBOSE:
                print(f"No CAN answer in {TIMEOUT_RCV} seconds")
            return "A:?, B:?"
            
    except Exception as e:
        if VERBOSE:
            print(f"Error in getasetup(): {e}")
        return "A:?, B:?"


# =============================================================================
# CAN INTERFACE CONTROL
# =============================================================================
def can_on() -> None:
    """
    Activa la interfície CAN a la Raspberry Pi.
    Configura el bitrate a 100kbps.
    """
    try:
        os.system(f'sudo /sbin/ip link set up {CAN_CHANNEL} type can bitrate {CAN_BITRATE}')
        sleep(0.1)
        if VERBOSE:
            print(f"CAN interface {CAN_CHANNEL} activated at {CAN_BITRATE} bps")
    except Exception as e:
        print(f"Error activating CAN interface: {e}")


def can_off() -> None:
    """
    Desactiva la interfície CAN.
    """
    try:
        # Tancar el bus abans de desactivar la interfície
        close_bus()
        os.system(f'sudo /sbin/ifconfig {CAN_CHANNEL} down')
        if VERBOSE:
            print(f"CAN interface {CAN_CHANNEL} deactivated")
    except Exception as e:
        print(f"Error deactivating CAN interface: {e}")


# =============================================================================
# MAIN - TEST CODE
# =============================================================================
if __name__ == "__main__":
    try:
        espera = 0.5
        can_on()
        
        # Test: Activar bits individualment en seqüència
        print("Testing bit-by-bit output...")
        for i in range(10):
            for bit in range(8):
                doutbit('0000', 'A', bit, 1)
                sleep(0.05)
                doutbit('0000', 'A', bit, 0)
        
        print("\nTest completed successfully!")
        
    except KeyboardInterrupt:
        print("\nInterrupted by user")
        dout('0000', 'A', 0x00)
        print("Digital output cleared")
        print(f"Digital input B: {din('0000', 'B')}")
        
    except Exception as e:
        print(f"Error during execution: {e}")
        
    finally:
        # Assegurar el tancament correcte
        close_bus()
        can_off()
        print("CAN bus closed and interface deactivated")