import protocol_kx
import can
import numpy as np
import struct
import time

K = np.zeros(protocol_kx.KX_K_COUNT)
K_received = np.zeros(protocol_kx.KX_K_COUNT)


class KX:
    def __init__(self, sbl_id, can, dict):
        self.sbl_id = sbl_id
        self.can0 = can
        for element in dict:
            K[element] = dict[element]

    def reset(self):
        resetMsg = can.Message(arbitration_id=0xA, data=[99, self.sbl_id], is_extended_id=False)
        try:
            self.can0.send(resetMsg)
        except self.can0.CanError:
            print("Message reset NOT sent")

    def sendK(self):
        i = 0
        for element in K:
            ba = bytearray(struct.pack("f", element))
            messageK = can.Message(arbitration_id=0x2BB, data=[self.sbl_id, i, 0,
                                                               protocol_kx.KX_FLAG_SEND, ba[0], ba[1], ba[2], ba[3]],
                                   is_extended_id=False)
            try:
                self.can0.send(messageK)
                time.sleep(0.01)
            except self.can0.CanError:
                print("Message sendK NOT sent")
            i = i + 1

    def writeToFlashK(self):
        messageK = can.Message(arbitration_id=0x2BB, data=[self.sbl_id, 0, 0,
                                                           protocol_kx.KX_FLAG_WRITE_REQ, 0, 0, 0, 0],
                               is_extended_id=False)
        try:
            self.can0.send(messageK)
            time.sleep(0.01)
        except self.can0.CanError:
            print("Flash NOT sent")

    def send_test_source_enable(self, enabaled):
        ba = bytearray(struct.pack("f", enabaled))
        messageK = can.Message(arbitration_id=0x2BB,
                               data=[self.sbl_id, protocol_kx.KDescription.K_TEST_SOURCES_ENABLED.value, 0,
                                     protocol_kx.KX_FLAG_SEND, ba[0], ba[1], ba[2], ba[3]], is_extended_id=False)
        try:
            self.can0.send(messageK)
            time.sleep(0.01)
        except self.can0.CanError:
            print("Test source enable NOT sent")

        self.sendK()
        self.writeToFlashK()

    def send_receive_cmd(self):
        messageK = can.Message(arbitration_id=0x2BB,
                               data=[self.sbl_id, 0, 0,
                                     protocol_kx.KX_FLAG_RECEIVE_REQ, 0, 0, 0, 0], is_extended_id=False)
        try:
            self.can0.send(messageK)
            time.sleep(0.01)
        except self.can0.CanError:
            print("Receive command NOT sent")

        d = 0
        while d < 64:
            message = self.can0.recv(10)
            if message.arbitration_id == 0x2BB:
                frame = bytearray()
                frame.append(message.data[4])
                frame.append(message.data[5])
                frame.append(message.data[6])
                frame.append(message.data[7])
                data= struct.unpack("f", frame)
                K_received[message.data[1]] = data[0]
                if(K_received[message.data[1]] != K[message.data[1]]):
                    print("Error in element ", message.data[1], "Need value = ", K[message.data[1]], "Get value = ", K_received[message.data[1]])
                d += 1