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


K = np.zeros(protocol_kx.KX_K_COUNT)


class KX:
    def __init__(self, id, value, sbl_id, can):
        K[id] = value
        self.sbl_id = sbl_id
        self.can0 = can


    def KXInit(self, id, value):
        K[id] = value


    def reset(self):
        resetMsg = can.Message(arbitration_id=0xA, data=[99, self.sbl_id], is_extended_id=False)
        try:
            self.can0.send(resetMsg)
            print(f"Message sent on{self.can0.channel_info}")
        except self.can0.CanError:
            print("Message 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)
                print(f"Message sent on{self.can0.channel_info}")
            except self.can0.CanError:
                print("Message 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)
            print(f"Flash sent on{self.can0.channel_info}")
        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)
        self.can0.send(messageK)
        self.sendK()
        self.writeToFlashK()