

import can
from ustruct import unpack_from,unpack,calcsize
from math import ceil

from PY_VTS import util
from PY_VTS.mgr.Files import *
from PY_VTS.mgr.Timers import *

class Msg:
    def __init__(self, id, data):
        self.id = id
        self.data = data

class MgrCAN:
    obj_count = 0
    bus_0_FrameCBs = {}
    bus_1_FrameCBs = {}
    bus0_t = None
    bus1_t = None

    signals = {}

    tx_list_bus_0 = {}
    tx_list_bus_1 = {}

    class FrameCB:
        def __init__(self, func, key, id):
            self.func = func
            self.key = key
            self.id = id

    def __init__(self, bus0=False, bus0_t=False, bus0_r = 500000, bus1=False, bus1_t=False, bus1_r = 500000):
        if MgrCAN.obj_count != 0:
            raise Exception('CAN Mgr: there can be only one')
        MgrCAN.obj_count += 1

        MgrCAN.bus0_t = bus0_t
        MgrCAN.bus1_t = bus1_t

        if not bus0 and not bus1:
            raise Exception('CAN Mgr: requires at least on CAN bus active')
        else:
            if bus0:
                can.reset(can.CAN0, bus0_r)
                can.termination(can.CAN0, bus0_t)
                can.test_mode(can.CAN0, 0)
            if bus1:
                can.reset(can.CAN1, bus1_r)
                can.termination(can.CAN1, bus1_t)
                can.test_mode(can.CAN1, 0)

        can.set_callback(can.CAN0, self.receive_bus_0_cb)
        can.set_callback(can.CAN1, self.receive_bus_1_cb)
        # Copy recieve callbacks stuff from TTM
        # set recieve_cb

    @classmethod
    def add_CAN_0_cb(cls, func, key, id):
        can.add_rx_id(can.CAN0, id)
        cls.bus_0_FrameCBs[key] = cls.FrameCB(func, key, id)

    @classmethod
    def rm_CAN_0_cb(key):
        del cls.bus_0_FrameCBs[key]

    @classmethod
    def add_CAN_1_cb(cls, func, key, id):
        can.add_rx_id(can.CAN1, id)
        cls.bus_1_FrameCBs[key] = cls.FrameCB(func, key, id)

    @classmethod
    def rm_CAN_1_cb(key):
        del cls.bus_1_FrameCBs[key]


    @classmethod
    def transmit(cls, id, msg, bus):
        if bus % 2 == 1: # Bus 0        if lowest bit set
            try:
                can.send_msg(can.CAN0,[id,False,msg])
            except TimeoutError:
                print("Timeout on Bus 0 (Upper port). Is it connected?")
        if (bus  >> 1) % 2: # Bus 1     if 2nd lowest bit set
            try:
                can.send_msg(can.CAN1,[id,False,msg])
            except TimeoutError:
                print("Timeout on Bus 1 (Lower port). Is it connected?")

    @classmethod
    def transmit_list(cls, period, bus, list): #list of CAN msgs
        if bus % 2 == 1:
            if not period in cls.tx_list_bus_0:
                cls.tx_list_bus_0[period] = []
                Mgr_Timers.add_clock(period = period)
                Mgr_Timers.Timers[period].add_cb("CAN_0", cls.Transmit_list_cb_0)
            cls.tx_list_bus_0[period].extend(list)
        if (bus  >> 1) % 2:
            if not period in cls.tx_list_bus_1:
                cls.tx_list_bus_1[period] = []
                Mgr_Timers.add_clock(period = period)
                Mgr_Timers.Timers[period].add_cb("CAN_1", cls.Transmit_list_cb_1)
            cls.tx_list_bus_1[period].extend(list)


    @classmethod
    def transmit_list_cb_0(cls, period):
        #period = str(period)
        if period in cls.tx_list_bus_0:
            try:
                msg = cls.tx_list_bus_0[period].pop(0)
            except IndexError:
                del cls.tx_list_bus_0[period]
                Mgr_Timers.Timers[500].rm_cb("CAN_0")
            else:
                cls.Transmit(msg.ID, msg.Data, 1)


    @classmethod
    def transmit_list_cb_1(cls, period):
        #period = str(period)
        if period in cls.tx_list_bus_1:
            try:
                msg = cls.tx_list_bus_1[period].pop(0)
            except IndexError:
                del cls.tx_list_bus_1[period]
                Mgr_Timers.Timers[500].rm_cb("CAN_1")
            else:
                cls.Transmit(msg.ID, msg.Data, 2)


    @classmethod
    def receive_bus_0_cb(cls):
        msg = can.get_msg(can.CAN0)
        for key, cb in cls.bus_0_FrameCBs.items():
            if cb.ID == msg.id:
                cb.func(msg)

    @classmethod
    def receive_bus_1_cb(cls):
        msg = can.get_msg(can.CAN1)
        for key, cb in cls.bus_1_FrameCBs.items():
            if cb.id == msg.id:
                cb.func(msg)

    @classmethod
    def set_termination(cls, bus):
        if bus % 2 == 1:
            can.termination(can.CAN0, True)
        else:
            can.termination(can.CAN0, False)
        if (bus  >> 1) % 2:
            can.termination(can.CAN1, True)
        else:
            can.termination(can.CAN1, False)

    @classmethod
    def add_signel(cls, key, signal, id, bus, start, length, msg_size = 8, type = "u", order = ">", scale = 1, offset = 0, min = 0, max = 0, enum = None, callback = None):
        cls.signals[key] = MgrCANSignal(key, signal, id, bus, start, length, msg_size, type, order, scale, offset, min, max, enum, callback)

    @classmethod
    def add_all_signals_from_dbc(cls, dbc, bus):
        for id in MgrFiles.files[dbc].Messags:
            for signal in Mgr_Files.files[dbc].Messags[id].signals:
                cls.add_signal_from_dbc(dbc, id+signal, int(id), signal, 3)

    @classmethod
    def add_signal_from_dbc(cls, dbc, key, id, signal, bus, cb = None):
        dbc = Mgr_Files.files[dbc]
        if dbc:
            if dbc.Messags[str(id)]:
                if dbc.Messags[str(id)].signals[signal]:
                    cls.add_signel(key, signal, id, bus,
                        dbc.Messags[str(id)].signals[signal].msb,
                        dbc.Messags[str(id)].signals[signal].len,
                        dbc.Messags[str(id)].len,
                        dbc.Messags[str(id)].signals[signal].type,
                        dbc.Messags[str(id)].signals[signal].endianness,
                        dbc.Messags[str(id)].signals[signal].mul,
                        dbc.Messags[str(id)].signals[signal].offset,
                        dbc.Messags[str(id)].signals[signal].min,
                        dbc.Messags[str(id)].signals[signal].max,
                        dbc.Messags[str(id)].signals[signal].enum,
                        cb
                    )
                else:
                    raise Exception("Signal not found in ID {}".format(str(id)))
            else:
                raise Exception("ID not found")
        else:
            raise Exception("DBC file needs to be loaded")

    @classmethod
    def remove_channel(key):
        del cls.channels[key]

class MgrCANSignal:

    def __init__(self, key, name, id, bus,  start, length, msg_size=8, type="u", order=">", scale=1, offset=0, min=0, max=0, enum=None, callback=None):

        self.name = name
        self.id = id
        self.bus = bus
        self.key = key
        self.msg_size = msg_size
        self.enum = enum

        self.start_byte = int(start // 8)

        self.num_bytes = (((start % 8) + length ) //8)


        self.start_shift = int(start % 8)
        self.start_mask = mask = 1 << self.start_shift #(7 if length > 7 else length)
        for j in range( 1, length if length < 8 - self.start_shift else 8 - self.start_shift):
            self.start_mask <<= 1
            self.start_mask += mask

        if ((start % 8) + length > 8):
            self.end_mask = 0
            self.end_shift = int((start + length) % 8)
            for j in range(0, self.end_shift):
                self.end_mask <<= 1
                self.end_mask += 1
        else:
            self.end_mask = 0
            self.end_shift = 0


        # for intel, little, <

        if order == ">" or order.lower() == "little" or order.lower() == "intel":    # little
            # proberly just swap start & end mask
            # if Length + Start > Msg_size * 8:
            #     raise Exception("{} signal overflows CAN message".format(Key))
            # if Start < 0:
            #     raise Exception("{} signal underflows CAN message".format(Key))

            temp = self.start_mask
            self.start_mask = self.end_mask
            self.end_mask = temp
            self.start_byte = self.start_byte + self.num_bytes -2
            self.order = "little"
            self.direction = -1
            self.num_bytes *= self.direction
            self.num_bytes += 1
            self.unpack = self.unpack_little
        elif order == "<" or order.lower() == "big" or order.lower() == "motorola":  # big
            # if Length + Start > (Msg_size - 1 * 16) + 8:
            #     raise Exception("{} signal o b verflows CAN message".format(Key))
            # if Start < 0:
            #     raise Exception("{} signal underflows CAN message".format(Key))
            self.start_byte = start // 8
            self.num_bytes = ceil(length / 8)
            if self.num_bytes == 1:
                letter = "B"
            elif self.num_bytes == 2:
                letter = "H"
            elif self.num_bytes == 4:
                letter = "L"
            elif self.num_bytes == 8:
                letter = "Q"

            self.string = ">{}s{}{}s".format(self.start_byte, letter, self.msg_size - self.start_byte - self.num_bytes  )
            self.order = "big"
            self.direction = 1
            self.unpack = self.unpack_big
        else:
            raise Exception("{} Endianness not reconised".format(order))


        # print("start:{} end:{} shift:{}".format(self.Start_mask ,self.End_mask, self.End_shift))

        self.length = length #- self.Start_bit
        self.order = order
        self.scale = scale
        self.offset = offset
        self.type = type
        self.callback = callback

        if min == 0 and max == 0:
            self.min_max = False
        else:
            self.min_max = True
            self.min = min
            self.max = max

        self.data = ""

        if self.type == "u":
            update = self.update_unsigned
        elif self.type == "s":
            update = self.update_signed
            self.twos_mask = 2 ** (length - 1)
        elif self.type == "f":
            update = self.update_float
        elif self.type == "d":
            update = self.update_double
        elif self.type == "e":
            update = self.update_enum

        if bus % 2 == 1: # Bus 0        if lowest bit set
            MgrCAN.add_CAN_0_cb(update, self.key, self.id)
        if (bus  >> 1) % 2: # Bus 1     if 2nd lowest bit set
            MgrCAN.add_CAN_1_cb(update, self.key, self.id)

    def unpack_big(self,msg):
        data_b = bytes(msg.data)
        data = unpack(self.string,data_b)
        return int(data[1])

    def unpack_little(self,msg):
        data = msg.data[self.start_byte] & self.start_mask
        data >>= self.start_shift
        for i in range(self.start_byte +1, self.start_byte + self.num_bytes, self.direction):
            data <<= 8
            data |= msg.data[i]
        if self.end_shift > 0:
            data <<= self.end_shift
            data |= msg.data[self.start_byte + self.num_bytes] & self.end_mask #self.Start_byte + self.Num_bytes]
        return int(data)

    def update_enum(self,msg):
        data = "{}".format(self.unpack(msg))
        if data in self.enum:
            self.data = self.enum["{}".format(data)]
        else:
            self.data = "{} undefined".format(data)
            print("{} in enum in {} signal on id {} is undefined".format(data, self.name, self.id))
        if self.callback:
            self.callback(self)

    def update_float(self,msg):
        data = self.unpack(msg)
        data = (unpack_from("f",data.to_bytes(4,"little"))[0] * self.scale) + self.offset
        if self.min_max:
            data = util.clamp(data, self.min, self.max)
        self.data = data
        if self.callback:
            self.callback(self)

    def update_double(self,msg):
        data = self.unpack(msg)
        data = (unpack_from("d",data.to_bytes(8,self.order))[0] * self.scale) + self.offset
        if self.min_max:
            data = util.clamp(data, self.min, self.max)
        self.data = data
        if self.callback:
            self.callback(self)

    # two's compliment
    def update_signed(self,msg):    # https://wiki.autosportlabs.com/CAN_Bus_Integration
        data = self.unpack(msg)
        data = -(data & self.twos_mask) + (data & ~self.twos_mask)
        data = (data * self.scale) + self.offset
        if self.min_max:
            data = util.clamp(data, self.min, self.max)
        self.data = data
        if self.callback:
            self.callback(self)

    def update_unsigned(self,msg):

        data = self.unpack(msg)
        data = (data * self.scale) + self.offset
        if self.min_max:
            data = util.clamp(data, self.min, self.max)
        self.data = data
        if self.callback:
            self.callback(self)
